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 
39 using namespace fawkes;
40 using 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 
66 void
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 
219 void
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 
236 void
237 OpenNiPointCloudThread::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 
264 void
265 OpenNiPointCloudThread::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 
294 void
295 OpenNiPointCloudThread::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 
331 void
332 OpenNiPointCloudThread::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
356 void
357 OpenNiPointCloudThread::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 
389 void
390 OpenNiPointCloudThread::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 
424 void
425 OpenNiPointCloudThread::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 
474 void
475 OpenNiPointCloudThread::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 
500 void
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 }
OpenNiImageThread
Definition: image_thread.h:46
OpenNiPointCloudThread::~OpenNiPointCloudThread
virtual ~OpenNiPointCloudThread()
Destructor.
Definition: pointcloud_thread.cpp:61
OpenNiPointCloudThread::init
virtual void init()
Initialize the thread.
Definition: pointcloud_thread.cpp:66
firevision::pcl_point_t::z
float z
Z value.
Definition: types.h:106
firevision::pcl_point_xyzrgb_t::g
uint8_t g
G color component value.
Definition: types.h:119
firevision::pcl_point_t::y
float y
Y value.
Definition: types.h:105
firevision::RGB_t::R
unsigned char R
R value.
Definition: rgb.h:68
firevision::pcl_point_t
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
Definition: types.h:102
firevision::RGB_t
Structure defining an RGB pixel (in R-G-B byte ordering).
Definition: rgb.h:66
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
firevision::pcl_point_xyzrgb_t::r
uint8_t r
R color component value.
Definition: types.h:120
firevision::SharedMemoryImageBuffer::set_capture_time
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:197
fawkes::SharedMemory::num_attached
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:767
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::MutexLocker
Definition: mutex_locker.h:37
fawkes::LockPtr::objmutex_ptr
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:297
fawkes::SharedMemory::unlock
void unlock()
Unlock memory.
Definition: shm.cpp:1029
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
firevision::SharedMemoryImageBuffer
Definition: shm_image.h:182
fawkes::Time::stamp_systime
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:725
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::ClockAspect::clock
Clock * clock
Definition: clock.h:53
firevision::SharedMemoryImageBuffer::set_frame_id
void set_frame_id(const char *frame_id)
Set frame ID.
Definition: shm_image.cpp:149
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes::SharedMemory::lock_for_write
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:963
firevision::pcl_point_xyzrgb_t::b
uint8_t b
B color component value.
Definition: types.h:118
firevision::pcl_point_t::x
float x
X value.
Definition: types.h:104
fawkes
pcl::PointCloud< pcl::PointXYZ >
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
firevision::pcl_point_xyzrgb_t::y
float y
Y value.
Definition: types.h:113
firevision::RGB_t::B
unsigned char B
B value.
Definition: rgb.h:70
firevision::SharedMemoryImageBuffer::buffer
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:227
OpenNiPointCloudThread::finalize
virtual void finalize()
Finalize the thread.
Definition: pointcloud_thread.cpp:219
fawkes::Time
Definition: time.h:96
OpenNiPointCloudThread::OpenNiPointCloudThread
OpenNiPointCloudThread(OpenNiImageThread *img_thread)
Constructor.
Definition: pointcloud_thread.cpp:53
fawkes::Thread::wait_loop_done
void wait_loop_done()
Wait for the current loop iteration to finish.
Definition: thread.cpp:1056
fawkes::Thread
Definition: thread.h:44
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
firevision::pcl_point_xyzrgb_t::z
float z
Z value.
Definition: types.h:114
firevision::pcl_point_xyzrgb_t
Structure defining a point in a CARTESIAN_3D_FLOAT_RGB buffer.
Definition: types.h:110
firevision::pcl_point_xyzrgb_t::x
float x
X value.
Definition: types.h:112
OpenNiPointCloudThread::loop
virtual void loop()
Code to execute in the thread.
Definition: pointcloud_thread.cpp:500
fawkes::OpenNiAspect::openni
LockPtr< xn::Context > openni
Definition: openni.h:46
firevision::RGB_t::G
unsigned char G
G value.
Definition: rgb.h:69
fawkes::Exception
Definition: exception.h:39