Fawkes API Fawkes Development Version
pointcloud_thread.cpp
1
2/***************************************************************************
3 * pointcloud_thread.cpp - OpenNI point cloud provider thread
4 *
5 * Created: Fri Mar 25 23:49:11 2011
6 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "pointcloud_thread.h"
24
25#include "image_thread.h"
26#include "utils/setup.h"
27
28#include <core/threading/mutex_locker.h>
29#include <fvutils/base/types.h>
30#include <fvutils/color/colorspaces.h>
31#include <fvutils/color/rgb.h>
32#include <fvutils/ipc/shm_image.h>
33#ifdef HAVE_PCL
34# include <pcl_utils/utils.h>
35#endif
36
37#include <memory>
38
39using namespace fawkes;
40using namespace firevision;
41
42/** @class OpenNiPointCloudThread "pointcloud_thread.h"
43 * OpenNI Point Cloud Provider Thread.
44 * This thread provides a point cloud calculated from the depth image
45 * acquired via OpenNI and provides it as a
46 * SharedMemoryImageBuffer to other FireVision plugins.
47 *
48 * @author Tim Niemueller
49 */
50
51/** Constructor.
52 * @param img_thread OpenNI image thread, used for XYZRGB point clouds
53 */
55: Thread("OpenNiPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
56 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
57{
58 img_thread_ = img_thread;
59}
60
61/** Destructor. */
63{
64}
65
66void
68{
70
71 image_rgb_buf_ = NULL;
72
73 depth_gen_ = new xn::DepthGenerator();
74#if __cplusplus >= 201103L
75 std::unique_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
76 std::unique_ptr<xn::ImageGenerator> imagegen_uniqueptr(image_gen_);
77#else
78 std::auto_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
79 std::auto_ptr<xn::ImageGenerator> imagegen_uniqueptr(image_gen_);
80#endif
81
82 image_gen_ = new xn::ImageGenerator();
83
84 XnStatus st;
85
86 fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, depth_gen_);
87 fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_IMAGE, image_gen_);
88 fawkes::openni::setup_map_generator(*image_gen_, config);
89 fawkes::openni::setup_map_generator(*depth_gen_, config);
90
91 depth_md_ = new xn::DepthMetaData();
92 depth_gen_->GetMetaData(*depth_md_);
93
94 cfg_register_depth_image_ = false;
95 try {
96 cfg_register_depth_image_ = config->get_bool("/plugins/openni/register_depth_image");
97 } catch (Exception &e) {
98 }
99
100 cfg_frame_depth_ = config->get_string("/plugins/openni/frame/depth");
101 cfg_frame_image_ = config->get_string("/plugins/openni/frame/image");
102
103 pcl_xyz_buf_ = new SharedMemoryImageBuffer("openni-pointcloud-xyz",
104 CARTESIAN_3D_FLOAT,
105 depth_md_->XRes(),
106 depth_md_->YRes());
107
108 pcl_xyz_buf_->set_frame_id(cfg_register_depth_image_ ? cfg_frame_image_.c_str()
109 : cfg_frame_depth_.c_str());
110
111 pcl_xyzrgb_buf_ = new SharedMemoryImageBuffer("openni-pointcloud-xyzrgb",
112 CARTESIAN_3D_FLOAT_RGB,
113 depth_md_->XRes(),
114 depth_md_->YRes());
115
116 pcl_xyzrgb_buf_->set_frame_id(cfg_register_depth_image_ ? cfg_frame_image_.c_str()
117 : cfg_frame_depth_.c_str());
118
119 // this is magic from ROS openni_device.cpp, reading code from
120 // openni-primesense suggests that SXGA is the base configuration
121 XnUInt64 zpd; // zero plane distance
122 if ((st = depth_gen_->GetIntProperty("ZPD", zpd)) != XN_STATUS_OK) {
123 throw Exception("Failed to get ZPD: %s", xnGetStatusString(st));
124 }
125 XnDouble pixel_size; // zero plane pixel size
126 if ((st = depth_gen_->GetRealProperty("ZPPS", pixel_size)) != XN_STATUS_OK) {
127 throw Exception("Failed to get ZPPS: %s", xnGetStatusString(st));
128 }
129
130 if ((st = depth_gen_->GetIntProperty("NoSampleValue", no_sample_value_)) != XN_STATUS_OK) {
131 throw Exception("Failed to get NoSampleValue: %s", xnGetStatusString(st));
132 }
133 if ((st = depth_gen_->GetIntProperty("ShadowValue", shadow_value_)) != XN_STATUS_OK) {
134 throw Exception("Failed to get ShadowValue: %s", xnGetStatusString(st));
135 }
136
137 width_ = depth_md_->XRes();
138 height_ = depth_md_->YRes();
139 float scale = width_ / (float)XN_SXGA_X_RES;
140 if (cfg_register_depth_image_) {
141 // magic number taken from ROS/PCL openni_device.cpp
142 const float rgb_focal_length_SXGA = 1050;
143 focal_length_ = rgb_focal_length_SXGA * scale;
144 } else {
145 focal_length_ = ((float)zpd / pixel_size) * scale;
146 }
147 foc_const_ = 0.001 / focal_length_;
148 center_x_ = (width_ / 2.) - .5f;
149 center_y_ = (height_ / 2.) - .5f;
150
151 image_gen_->StartGenerating();
152 depth_gen_->StartGenerating();
153
154 capture_start_ = new Time(clock);
155 capture_start_->stamp_systime();
156 // Update once to get timestamp
157 depth_gen_->WaitAndUpdateData();
158 // arbitrarily define the zero reference point,
159 // we can't get any closer than this
160 *capture_start_ -= (long int)depth_gen_->GetTimestamp();
161
162 image_gen_->WaitAndUpdateData();
163
164 if (cfg_register_depth_image_) {
165 // RegistrationType should be 2 (software) for Kinect, 1 (hardware) for PS
166 // (from ROS openni_camera)
167 unsigned short usb_vendor = 0, usb_product = 0;
168 fawkes::openni::get_usb_info(*depth_gen_, usb_vendor, usb_product);
169
170 if ((usb_vendor == 0x045e) && (usb_product == 0x02ae)) {
171 if (depth_gen_->SetIntProperty("RegistrationType", 2) != XN_STATUS_OK) {
172 throw Exception("Failed to set registration type");
173 }
174 } else {
175 if (depth_gen_->SetIntProperty("RegistrationType", 1) != XN_STATUS_OK) {
176 throw Exception("Failed to set registration type");
177 }
178 }
179
180 logger->log_info(name(), "Setting depth alternate viewpoint to image");
181 fawkes::openni::setup_alternate_viewpoint(*depth_gen_, *image_gen_);
182 }
183
184 // Fails with "Bad Paramter" on OpenNI 1.3.2.1/PS 5.0.3.3
185 //logger->log_info(name(), "Setting depth/image synchronization");
186 //fawkes::openni::setup_synchronization(*depth_gen_, *image_gen_);
187
188#ifdef HAVE_PCL
189 cfg_generate_pcl_ = true;
190 try {
191 cfg_generate_pcl_ = config->get_bool("/plugins/openni-pointcloud/generate-pcl");
192 } catch (Exception &e) {
193 }
194
195 if (cfg_generate_pcl_) {
196 pcl_xyz_ = new pcl::PointCloud<pcl::PointXYZ>();
197 pcl_xyz_->is_dense = false;
198 pcl_xyz_->width = width_;
199 pcl_xyz_->height = height_;
200 pcl_xyz_->points.resize((size_t)width_ * (size_t)height_);
201 pcl_xyz_->header.frame_id = cfg_register_depth_image_ ? cfg_frame_image_ : cfg_frame_depth_;
202
203 pcl_xyzrgb_ = new pcl::PointCloud<pcl::PointXYZRGB>();
204 pcl_xyzrgb_->is_dense = false;
205 pcl_xyzrgb_->width = width_;
206 pcl_xyzrgb_->height = height_;
207 pcl_xyzrgb_->points.resize((size_t)width_ * (size_t)height_);
208 pcl_xyzrgb_->header.frame_id = cfg_register_depth_image_ ? cfg_frame_image_ : cfg_frame_depth_;
209
210 pcl_manager->add_pointcloud("openni-pointcloud-xyz", pcl_xyz_);
211 pcl_manager->add_pointcloud("openni-pointcloud-xyzrgb", pcl_xyzrgb_);
212 }
213#endif
214
215 depthgen_uniqueptr.release();
216 imagegen_uniqueptr.release();
217}
218
219void
221{
222#ifdef HAVE_PCL
223 pcl_manager->remove_pointcloud("openni-pointcloud-xyz");
224 pcl_manager->remove_pointcloud("openni-pointcloud-xyzrgb");
225#endif
226
227 // we do not stop generating, we don't know if there is no other plugin
228 // using the node.
229 delete depth_gen_;
230 delete depth_md_;
231 delete pcl_xyz_buf_;
232 delete pcl_xyzrgb_buf_;
233 delete capture_start_;
234}
235
236void
237OpenNiPointCloudThread::fill_xyz_no_pcl(fawkes::Time &ts, const XnDepthPixel *const depth_data)
238{
239 pcl_xyz_buf_->lock_for_write();
240 pcl_xyz_buf_->set_capture_time(&ts);
241
242 pcl_point_t *pclbuf = (pcl_point_t *)pcl_xyz_buf_->buffer();
243
244 unsigned int idx = 0;
245 for (unsigned int h = 0; h < height_; ++h) {
246 for (unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf) {
247 // Check for invalid measurements
248 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
249 || depth_data[idx] == shadow_value_) {
250 // invalid
251 pclbuf->x = pclbuf->y = pclbuf->z = 0.f;
252 } else {
253 // Fill in XYZ
254 pclbuf->x = depth_data[idx] * 0.001f;
255 pclbuf->y = -(w - center_x_) * depth_data[idx] * foc_const_;
256 pclbuf->z = -(h - center_y_) * depth_data[idx] * foc_const_;
257 }
258 }
259 }
260
261 pcl_xyz_buf_->unlock();
262}
263
264void
265OpenNiPointCloudThread::fill_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel *const depth_data)
266{
267 pcl_xyzrgb_buf_->lock_for_write();
268 pcl_xyzrgb_buf_->set_capture_time(&ts);
269
270 pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)pcl_xyzrgb_buf_->buffer();
271
272 unsigned int idx = 0;
273 for (unsigned int h = 0; h < height_; ++h) {
274 for (unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb) {
275 // Check for invalid measurements
276 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
277 || depth_data[idx] == shadow_value_) {
278 // invalid
279 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
280 } else {
281 // Fill in XYZ
282 pclbuf_rgb->x = depth_data[idx] * 0.001f;
283 pclbuf_rgb->y = -(w - center_x_) * depth_data[idx] * foc_const_;
284 pclbuf_rgb->z = -(h - center_y_) * depth_data[idx] * foc_const_;
285 }
286 }
287 }
288
289 fill_rgb_no_pcl();
290
291 pcl_xyzrgb_buf_->unlock();
292}
293
294void
295OpenNiPointCloudThread::fill_xyz_xyzrgb_no_pcl(fawkes::Time & ts,
296 const XnDepthPixel *const depth_data)
297{
298 pcl_xyz_buf_->lock_for_write();
299 pcl_xyz_buf_->set_capture_time(&ts);
300
301 pcl_xyzrgb_buf_->lock_for_write();
302 pcl_xyzrgb_buf_->set_capture_time(&ts);
303
304 pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)pcl_xyzrgb_buf_->buffer();
305 pcl_point_t * pclbuf_xyz = (pcl_point_t *)pcl_xyz_buf_->buffer();
306
307 unsigned int idx = 0;
308 for (unsigned int h = 0; h < height_; ++h) {
309 for (unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
310 // Check for invalid measurements
311 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
312 || depth_data[idx] == shadow_value_) {
313 // invalid
314 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
315 pclbuf_xyz->x = pclbuf_xyz->y = pclbuf_xyz->z = 0.f;
316 } else {
317 // Fill in XYZ
318 pclbuf_rgb->x = pclbuf_xyz->x = depth_data[idx] * 0.001f;
319 pclbuf_rgb->y = pclbuf_xyz->y = -(w - center_x_) * depth_data[idx] * foc_const_;
320 pclbuf_rgb->z = pclbuf_xyz->z = -(h - center_y_) * depth_data[idx] * foc_const_;
321 }
322 }
323 }
324
325 fill_rgb_no_pcl();
326
327 pcl_xyzrgb_buf_->unlock();
328 pcl_xyz_buf_->unlock();
329}
330
331void
332OpenNiPointCloudThread::fill_rgb_no_pcl()
333{
334 if (!image_rgb_buf_) {
335 try {
336 image_rgb_buf_ = new SharedMemoryImageBuffer("openni-image-rgb");
337 } catch (Exception &e) {
338 logger->log_warn(name(), "Failed to open openni-image-rgb shm image buffer");
339 return;
340 }
341 }
342
343 img_thread_->wait_loop_done();
344
345 pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)pcl_xyzrgb_buf_->buffer();
346 RGB_t * imagebuf = (RGB_t *)image_rgb_buf_->buffer();
347
348 for (unsigned int i = 0; i < width_ * height_; ++i) {
349 pclbuf_rgb->r = imagebuf[i].R;
350 pclbuf_rgb->g = imagebuf[i].G;
351 pclbuf_rgb->b = imagebuf[i].B;
352 }
353}
354
355#ifdef HAVE_PCL
356void
357OpenNiPointCloudThread::fill_xyz(fawkes::Time &ts, const XnDepthPixel *const depth_data)
358{
359 pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_xyz_;
360 pcl.header.seq += 1;
361 pcl_utils::set_time(pcl_xyz_, ts);
362
363 pcl_xyz_buf_->lock_for_write();
364 pcl_xyz_buf_->set_capture_time(&ts);
365
366 pcl_point_t *pclbuf = (pcl_point_t *)pcl_xyz_buf_->buffer();
367
368 unsigned int idx = 0;
369 for (unsigned int h = 0; h < height_; ++h) {
370 for (unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf) {
371 // Check for invalid measurements
372 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
373 || depth_data[idx] == shadow_value_) {
374 // invalid
375 pclbuf->x = pclbuf->y = pclbuf->z = 0.f;
376 pcl.points[idx].x = pcl.points[idx].y = pcl.points[idx].z = 0.f;
377 } else {
378 // Fill in XYZ
379 pclbuf->x = pcl.points[idx].x = depth_data[idx] * 0.001f;
380 pclbuf->y = pcl.points[idx].y = -(w - center_x_) * depth_data[idx] * foc_const_;
381 pclbuf->z = pcl.points[idx].z = -(h - center_y_) * depth_data[idx] * foc_const_;
382 }
383 }
384 }
385
386 pcl_xyz_buf_->unlock();
387}
388
389void
390OpenNiPointCloudThread::fill_xyzrgb(fawkes::Time &ts, const XnDepthPixel *const depth_data)
391{
392 pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb = **pcl_xyzrgb_;
393 pcl_rgb.header.seq += 1;
394 pcl_utils::set_time(pcl_xyzrgb_, ts);
395
396 pcl_xyzrgb_buf_->lock_for_write();
397 pcl_xyzrgb_buf_->set_capture_time(&ts);
398
399 pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)pcl_xyzrgb_buf_->buffer();
400
401 unsigned int idx = 0;
402 for (unsigned int h = 0; h < height_; ++h) {
403 for (unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb) {
404 // Check for invalid measurements
405 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
406 || depth_data[idx] == shadow_value_) {
407 // invalid
408 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
409 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
410 } else {
411 // Fill in XYZ
412 pclbuf_rgb->x = pcl_rgb.points[idx].x = depth_data[idx] * 0.001f;
413 pclbuf_rgb->y = pcl_rgb.points[idx].y = -(w - center_x_) * depth_data[idx] * foc_const_;
414 pclbuf_rgb->z = pcl_rgb.points[idx].z = -(h - center_y_) * depth_data[idx] * foc_const_;
415 }
416 }
417 }
418
419 fill_rgb(pcl_rgb);
420
421 pcl_xyzrgb_buf_->unlock();
422}
423
424void
425OpenNiPointCloudThread::fill_xyz_xyzrgb(fawkes::Time &ts, const XnDepthPixel *const depth_data)
426{
427 pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb = **pcl_xyzrgb_;
428 pcl_rgb.header.seq += 1;
429 pcl_utils::set_time(pcl_xyzrgb_, ts);
430
431 pcl::PointCloud<pcl::PointXYZ> &pcl_xyz = **pcl_xyz_;
432 pcl_xyz.header.seq += 1;
433 pcl_utils::set_time(pcl_xyz_, ts);
434
435 pcl_xyz_buf_->lock_for_write();
436 pcl_xyz_buf_->set_capture_time(&ts);
437
438 pcl_xyzrgb_buf_->lock_for_write();
439 pcl_xyzrgb_buf_->set_capture_time(&ts);
440
441 pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)pcl_xyzrgb_buf_->buffer();
442 pcl_point_t * pclbuf_xyz = (pcl_point_t *)pcl_xyz_buf_->buffer();
443
444 unsigned int idx = 0;
445 for (unsigned int h = 0; h < height_; ++h) {
446 for (unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
447 // Check for invalid measurements
448 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
449 || depth_data[idx] == shadow_value_) {
450 // invalid
451 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
452 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
453
454 pclbuf_xyz->x = pclbuf_xyz->y = pclbuf_xyz->z = 0.f;
455 pcl_xyz.points[idx].x = pcl_xyz.points[idx].y = pcl_xyz.points[idx].z = 0.f;
456 } else {
457 // Fill in XYZ
458 pclbuf_rgb->x = pcl_rgb.points[idx].x = pclbuf_xyz->x = pcl_xyz.points[idx].x =
459 depth_data[idx] * 0.001f;
460 pclbuf_rgb->y = pcl_rgb.points[idx].y = pclbuf_xyz->y = pcl_xyz.points[idx].y =
461 -(w - center_x_) * depth_data[idx] * foc_const_;
462 pclbuf_rgb->z = pcl_rgb.points[idx].z = pclbuf_xyz->z = pcl_xyz.points[idx].z =
463 -(h - center_y_) * depth_data[idx] * foc_const_;
464 }
465 }
466 }
467
468 fill_rgb(pcl_rgb);
469
470 pcl_xyzrgb_buf_->unlock();
471 pcl_xyz_buf_->unlock();
472}
473
474void
475OpenNiPointCloudThread::fill_rgb(pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb)
476{
477 if (!image_rgb_buf_) {
478 try {
479 image_rgb_buf_ = new SharedMemoryImageBuffer("openni-image-rgb");
480 } catch (Exception &e) {
481 logger->log_warn(name(), "Failed to open openni-image-rgb shm image buffer");
482 return;
483 }
484 }
485
486 img_thread_->wait_loop_done();
487
488 pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)pcl_xyzrgb_buf_->buffer();
489 RGB_t * imagebuf = (RGB_t *)image_rgb_buf_->buffer();
490
491 for (unsigned int i = 0; i < width_ * height_; ++i) {
492 pclbuf_rgb->r = pcl_rgb.points[i].r = imagebuf[i].R;
493 pclbuf_rgb->g = pcl_rgb.points[i].g = imagebuf[i].G;
494 pclbuf_rgb->b = pcl_rgb.points[i].b = imagebuf[i].B;
495 }
496}
497
498#endif
499
500void
502{
504 bool is_data_new = depth_gen_->IsDataNew();
505 depth_gen_->GetMetaData(*depth_md_);
506 const XnDepthPixel *const data = depth_md_->Data();
507 // experimental: unlock here as we do not invoke any methods anymore
508 // since data has been updated earlier in the sensor hook we should be safe
509 lock.unlock();
510
511 bool xyz_requested = (pcl_xyz_buf_->num_attached() > 1)
512#ifdef HAVE_PCL
513 // 2 is us and the PCL manager of the PointCloudAspect
514 || (cfg_generate_pcl_ && ((pcl_xyz_.use_count() > 2)))
515#endif
516 ;
517 bool xyzrgb_requested = (pcl_xyzrgb_buf_->num_attached() > 1)
518#ifdef HAVE_PCL
519 // 2 is us and the PCL manager of the PointCloudAspect
520 || (cfg_generate_pcl_ && ((pcl_xyzrgb_.use_count() > 2)))
521#endif
522 ;
523
524 if (is_data_new && (xyz_requested || xyzrgb_requested)) {
525 // convert depth to points
526 fawkes::Time ts = *capture_start_ + (long int)depth_gen_->GetTimestamp();
527
528#ifdef HAVE_PCL
529 if (cfg_generate_pcl_) {
530 if (xyz_requested && xyzrgb_requested) {
531 fill_xyz_xyzrgb(ts, data);
532 } else if (xyz_requested) {
533 fill_xyz(ts, data);
534 } else if (xyzrgb_requested) {
535 fill_xyzrgb(ts, data);
536 }
537
538 } else {
539#endif
540 if (xyz_requested && xyzrgb_requested) {
541 fill_xyz_xyzrgb_no_pcl(ts, data);
542 } else if (xyz_requested) {
543 fill_xyz_no_pcl(ts, data);
544 } else if (xyzrgb_requested) {
545 fill_xyzrgb_no_pcl(ts, data);
546 }
547#ifdef HAVE_PCL
548 }
549#endif
550
551 // close rgb image buffer if no longer required
552 if (!xyzrgb_requested && image_rgb_buf_) {
553 delete image_rgb_buf_;
554 image_rgb_buf_ = NULL;
555 }
556 }
557}
OpenNI Image Provider Thread.
Definition: image_thread.h:53
OpenNiPointCloudThread(OpenNiImageThread *img_thread)
Constructor.
virtual void init()
Initialize the thread.
virtual ~OpenNiPointCloudThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
void unlock()
Unlock the mutex.
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:47
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:959
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:763
void unlock()
Unlock memory.
Definition: shm.cpp:1025
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void wait_loop_done()
Wait for the current loop iteration to finish.
Definition: thread.cpp:1052
A class for handling time.
Definition: time.h:93
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:720
Shared memory image buffer.
Definition: shm_image.h:184
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:198
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:228
void set_frame_id(const char *frame_id)
Set frame ID.
Definition: shm_image.cpp:150
Fawkes library namespace.
Structure defining an RGB pixel (in R-G-B byte ordering).
Definition: rgb.h:62
unsigned char G
G value.
Definition: rgb.h:64
unsigned char R
R value.
Definition: rgb.h:63
unsigned char B
B value.
Definition: rgb.h:65
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
Definition: types.h:98
float z
Z value.
Definition: types.h:101
float x
X value.
Definition: types.h:99
float y
Y value.
Definition: types.h:100
Structure defining a point in a CARTESIAN_3D_FLOAT_RGB buffer.
Definition: types.h:106
uint8_t r
R color component value.
Definition: types.h:115
uint8_t g
G color component value.
Definition: types.h:114
uint8_t b
B color component value.
Definition: types.h:113