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