Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 #define FRAME_ID_DEPTH "/kinect/depth"
42 #define FRAME_ID_IMAGE "/kinect/image"
43 
44 /** @class OpenNiPointCloudThread "pointcloud_thread.h"
45  * OpenNI Point Cloud Provider Thread.
46  * This thread provides a point cloud calculated from the depth image
47  * acquired via OpenNI and provides it as a
48  * SharedMemoryImageBuffer to other FireVision plugins.
49  *
50  * @author Tim Niemueller
51  */
52 
53 /** Constructor.
54  * @param img_thread OpenNI image thread, used for XYZRGB point clouds
55  */
57  : Thread("OpenNiPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
58  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
59 {
60  __img_thread = img_thread;
61 }
62 
63 
64 /** Destructor. */
66 {
67 }
68 
69 
70 void
72 {
74 
75  __image_rgb_buf = NULL;
76 
77  __depth_gen = new xn::DepthGenerator();
78  std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
79  __image_gen = new xn::ImageGenerator();
80  std::auto_ptr<xn::ImageGenerator> imagegen_autoptr(__image_gen);
81 
82  XnStatus st;
83 
84  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, __depth_gen);
85  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_IMAGE, __image_gen);
86  fawkes::openni::setup_map_generator(*__image_gen, config);
87  fawkes::openni::setup_map_generator(*__depth_gen, config);
88 
89  __depth_md = new xn::DepthMetaData();
90  __depth_gen->GetMetaData(*__depth_md);
91 
92  __cfg_register_depth_image = false;
93  try {
94  __cfg_register_depth_image = config->get_bool("/plugins/openni/register_depth_image");
95  } catch (Exception &e) {}
96 
97  __pcl_xyz_buf = new SharedMemoryImageBuffer("openni-pointcloud-xyz",
98  CARTESIAN_3D_FLOAT,
99  __depth_md->XRes(), __depth_md->YRes());
100 
101  __pcl_xyz_buf->set_frame_id(__cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH);
102 
103 
104  __pcl_xyzrgb_buf = new SharedMemoryImageBuffer("openni-pointcloud-xyzrgb",
105  CARTESIAN_3D_FLOAT_RGB,
106  __depth_md->XRes(), __depth_md->YRes());
107 
108  __pcl_xyzrgb_buf->set_frame_id(__cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH);
109 
110  // this is magic from ROS openni_device.cpp, reading code from
111  // openni-primesense suggests that SXGA is the base configuration
112  XnUInt64 zpd; // zero plane distance
113  if ((st = __depth_gen->GetIntProperty("ZPD", zpd)) != XN_STATUS_OK) {
114  throw Exception("Failed to get ZPD: %s", xnGetStatusString(st));
115  }
116  XnDouble pixel_size; // zero plane pixel size
117  if ((st = __depth_gen->GetRealProperty("ZPPS", pixel_size)) != XN_STATUS_OK) {
118  throw Exception("Failed to get ZPPS: %s", xnGetStatusString(st));
119  }
120 
121  if ((st = __depth_gen->GetIntProperty("NoSampleValue", __no_sample_value))
122  != XN_STATUS_OK)
123  {
124  throw Exception("Failed to get NoSampleValue: %s", xnGetStatusString(st));
125  }
126  if ((st = __depth_gen->GetIntProperty("ShadowValue", __shadow_value))
127  != XN_STATUS_OK)
128  {
129  throw Exception("Failed to get ShadowValue: %s", xnGetStatusString(st));
130  }
131 
132  __width = __depth_md->XRes();
133  __height = __depth_md->YRes();
134  float scale = __width / (float)XN_SXGA_X_RES;
135  if (__cfg_register_depth_image) {
136  // magic number taken from ROS/PCL openni_device.cpp
137  const float rgb_focal_length_SXGA = 1050;
138  __focal_length = rgb_focal_length_SXGA * scale;
139  } else {
140  __focal_length = ((float)zpd / pixel_size) * scale;
141  }
142  __foc_const = 0.001 / __focal_length;
143  __center_x = (__width / 2.) - .5f;
144  __center_y = (__height / 2.) - .5f;
145 
146  __image_gen->StartGenerating();
147  __depth_gen->StartGenerating();
148 
149  __capture_start = new Time(clock);
150  __capture_start->stamp_systime();
151  // Update once to get timestamp
152  __depth_gen->WaitAndUpdateData();
153  // arbitrarily define the zero reference point,
154  // we can't get any closer than this
155  *__capture_start -= (long int)__depth_gen->GetTimestamp();
156 
157  __image_gen->WaitAndUpdateData();
158 
159  if (__cfg_register_depth_image) {
160  logger->log_info(name(), "Setting depth alternate viewpoint to image");
161  fawkes::openni::setup_alternate_viewpoint(*__depth_gen, *__image_gen);
162  }
163 
164  // Fails with "Bad Paramter" on OpenNI 1.3.2.1/PS 5.0.3.3
165  //logger->log_info(name(), "Setting depth/image synchronization");
166  //fawkes::openni::setup_synchronization(*__depth_gen, *__image_gen);
167 
168 #ifdef HAVE_PCL
169  __cfg_generate_pcl = true;
170  try {
171  __cfg_generate_pcl = config->get_bool("/plugins/openni-pointcloud/generate-pcl");
172  } catch (Exception &e) {}
173 
174  if (__cfg_generate_pcl) {
175  __pcl_xyz = new pcl::PointCloud<pcl::PointXYZ>();
176  __pcl_xyz->is_dense = false;
177  __pcl_xyz->width = __width;
178  __pcl_xyz->height = __height;
179  __pcl_xyz->points.resize(__width * __height);
180  __pcl_xyz->header.frame_id = __cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH;
181 
182  __pcl_xyzrgb = new pcl::PointCloud<pcl::PointXYZRGB>();
183  __pcl_xyzrgb->is_dense = false;
184  __pcl_xyzrgb->width = __width;
185  __pcl_xyzrgb->height = __height;
186  __pcl_xyzrgb->points.resize(__width * __height);
187  __pcl_xyzrgb->header.frame_id = __cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH;
188 
189  pcl_manager->add_pointcloud("openni-pointcloud-xyz", __pcl_xyz);
190  pcl_manager->add_pointcloud("openni-pointcloud-xyzrgb", __pcl_xyzrgb);
191  }
192 #endif
193 
194  depthgen_autoptr.release();
195  imagegen_autoptr.release();
196 }
197 
198 
199 void
201 {
202 #ifdef HAVE_PCL
203  pcl_manager->remove_pointcloud("openni-pointcloud-xyz");
204  pcl_manager->remove_pointcloud("openni-pointcloud-xyzrgb");
205 #endif
206 
207  // we do not stop generating, we don't know if there is no other plugin
208  // using the node.
209  delete __depth_gen;
210  delete __depth_md;
211  delete __pcl_xyz_buf;
212  delete __pcl_xyzrgb_buf;
213  delete __capture_start;
214 }
215 
216 
217 void
218 OpenNiPointCloudThread::fill_xyz_no_pcl(fawkes::Time &ts, const XnDepthPixel * const depth_data)
219 {
220  __pcl_xyz_buf->lock_for_write();
221  __pcl_xyz_buf->set_capture_time(&ts);
222 
223  register pcl_point_t *pclbuf = (pcl_point_t *)__pcl_xyz_buf->buffer();
224 
225  unsigned int idx = 0;
226  for (unsigned int h = 0; h < __height; ++h) {
227  for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
228  // Check for invalid measurements
229  if (depth_data[idx] == 0 ||
230  depth_data[idx] == __no_sample_value ||
231  depth_data[idx] == __shadow_value)
232  {
233  // invalid
234  pclbuf->x = pclbuf->y = pclbuf->z = 0.f;
235  } else {
236  // Fill in XYZ
237  pclbuf->x = depth_data[idx] * 0.001f;
238  pclbuf->y = -(w - __center_x) * depth_data[idx] * __foc_const;
239  pclbuf->z = -(h - __center_y) * depth_data[idx] * __foc_const;
240  }
241  }
242  }
243 
244  __pcl_xyz_buf->unlock();
245 }
246 
247 
248 void
249 OpenNiPointCloudThread::fill_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const depth_data)
250 {
251  __pcl_xyzrgb_buf->lock_for_write();
252  __pcl_xyzrgb_buf->set_capture_time(&ts);
253 
254  register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
255 
256  unsigned int idx = 0;
257  for (unsigned int h = 0; h < __height; ++h) {
258  for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
259  // Check for invalid measurements
260  if (depth_data[idx] == 0 ||
261  depth_data[idx] == __no_sample_value ||
262  depth_data[idx] == __shadow_value)
263  {
264  // invalid
265  pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
266  } else {
267  // Fill in XYZ
268  pclbuf_rgb->x = depth_data[idx] * 0.001f;
269  pclbuf_rgb->y = -(w - __center_x) * depth_data[idx] * __foc_const;
270  pclbuf_rgb->z = -(h - __center_y) * depth_data[idx] * __foc_const;
271  }
272  }
273  }
274 
275  fill_rgb_no_pcl();
276 
277  __pcl_xyzrgb_buf->unlock();
278 }
279 
280 
281 void
282 OpenNiPointCloudThread::fill_xyz_xyzrgb_no_pcl(fawkes::Time &ts,
283  const XnDepthPixel * const depth_data)
284 {
285  __pcl_xyz_buf->lock_for_write();
286  __pcl_xyz_buf->set_capture_time(&ts);
287 
288  __pcl_xyzrgb_buf->lock_for_write();
289  __pcl_xyzrgb_buf->set_capture_time(&ts);
290 
291  register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
292  register pcl_point_t *pclbuf_xyz = (pcl_point_t *)__pcl_xyz_buf->buffer();
293 
294  unsigned int idx = 0;
295  for (unsigned int h = 0; h < __height; ++h) {
296  for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
297  // Check for invalid measurements
298  if (depth_data[idx] == 0 ||
299  depth_data[idx] == __no_sample_value ||
300  depth_data[idx] == __shadow_value)
301  {
302  // invalid
303  pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
304  pclbuf_xyz->x = pclbuf_xyz->y = pclbuf_xyz->z = 0.f;
305  } else {
306  // Fill in XYZ
307  pclbuf_rgb->x = pclbuf_xyz->x = depth_data[idx] * 0.001f;
308  pclbuf_rgb->y = pclbuf_xyz->y = -(w - __center_x) * depth_data[idx] * __foc_const;
309  pclbuf_rgb->z = pclbuf_xyz->z = -(h - __center_y) * depth_data[idx] * __foc_const;
310  }
311  }
312  }
313 
314  fill_rgb_no_pcl();
315 
316  __pcl_xyzrgb_buf->unlock();
317  __pcl_xyz_buf->unlock();
318 }
319 
320 
321 void
322 OpenNiPointCloudThread::fill_rgb_no_pcl()
323 {
324  if (! __image_rgb_buf) {
325  try {
326  __image_rgb_buf = new SharedMemoryImageBuffer("openni-image-rgb");
327  } catch (Exception &e) {
328  logger->log_warn(name(), "Failed to open openni-image-rgb shm image buffer");
329  return;
330  }
331  }
332 
333  __img_thread->wait_loop_done();
334 
335  register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
336  register RGB_t *imagebuf = (RGB_t *)__image_rgb_buf->buffer();
337 
338  for (unsigned int i = 0; i < __width * __height; ++i) {
339  pclbuf_rgb->r = imagebuf[i].R;
340  pclbuf_rgb->g = imagebuf[i].G;
341  pclbuf_rgb->b = imagebuf[i].B;
342  }
343 }
344 
345 
346 
347 #ifdef HAVE_PCL
348 void
349 OpenNiPointCloudThread::fill_xyz(fawkes::Time &ts, const XnDepthPixel * const depth_data)
350 {
351  pcl::PointCloud<pcl::PointXYZ> &pcl = **__pcl_xyz;
352  pcl.header.seq += 1;
353  pcl_utils::set_time(__pcl_xyz, ts);
354 
355  __pcl_xyz_buf->lock_for_write();
356  __pcl_xyz_buf->set_capture_time(&ts);
357 
358  register pcl_point_t *pclbuf = (pcl_point_t *)__pcl_xyz_buf->buffer();
359 
360  unsigned int idx = 0;
361  for (unsigned int h = 0; h < __height; ++h) {
362  for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
363  // Check for invalid measurements
364  if (depth_data[idx] == 0 ||
365  depth_data[idx] == __no_sample_value ||
366  depth_data[idx] == __shadow_value)
367  {
368  // invalid
369  pclbuf->x = pclbuf->y = pclbuf->z = 0.f;
370  pcl.points[idx].x = pcl.points[idx].y = pcl.points[idx].z = 0.f;
371  } else {
372  // Fill in XYZ
373  pclbuf->x = pcl.points[idx].x = depth_data[idx] * 0.001f;
374  pclbuf->y = pcl.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
375  pclbuf->z = pcl.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
376  }
377  }
378  }
379 
380  __pcl_xyz_buf->unlock();
381 }
382 
383 
384 void
385 OpenNiPointCloudThread::fill_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data)
386 {
387  pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb = **__pcl_xyzrgb;
388  pcl_rgb.header.seq += 1;
389  pcl_utils::set_time(__pcl_xyzrgb, ts);
390 
391  __pcl_xyzrgb_buf->lock_for_write();
392  __pcl_xyzrgb_buf->set_capture_time(&ts);
393 
394  register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
395 
396  unsigned int idx = 0;
397  for (unsigned int h = 0; h < __height; ++h) {
398  for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
399  // Check for invalid measurements
400  if (depth_data[idx] == 0 ||
401  depth_data[idx] == __no_sample_value ||
402  depth_data[idx] == __shadow_value)
403  {
404  // invalid
405  pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
406  pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
407  } else {
408  // Fill in XYZ
409  pclbuf_rgb->x = pcl_rgb.points[idx].x = depth_data[idx] * 0.001f;
410  pclbuf_rgb->y = pcl_rgb.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
411  pclbuf_rgb->z = pcl_rgb.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
412  }
413  }
414  }
415 
416  fill_rgb(pcl_rgb);
417 
418  __pcl_xyzrgb_buf->unlock();
419 }
420 
421 
422 void
423 OpenNiPointCloudThread::fill_xyz_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data)
424 {
425 
426  pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb = **__pcl_xyzrgb;
427  pcl_rgb.header.seq += 1;
428  pcl_utils::set_time(__pcl_xyzrgb, ts);
429 
430  pcl::PointCloud<pcl::PointXYZ> &pcl_xyz = **__pcl_xyz;
431  pcl_xyz.header.seq += 1;
432  pcl_utils::set_time(__pcl_xyz, ts);
433 
434  __pcl_xyz_buf->lock_for_write();
435  __pcl_xyz_buf->set_capture_time(&ts);
436 
437  __pcl_xyzrgb_buf->lock_for_write();
438  __pcl_xyzrgb_buf->set_capture_time(&ts);
439 
440  register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
441  register pcl_point_t *pclbuf_xyz = (pcl_point_t *)__pcl_xyz_buf->buffer();
442 
443  unsigned int idx = 0;
444  for (unsigned int h = 0; h < __height; ++h) {
445  for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
446  // Check for invalid measurements
447  if (depth_data[idx] == 0 ||
448  depth_data[idx] == __no_sample_value ||
449  depth_data[idx] == __shadow_value)
450  {
451  // invalid
452  pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
453  pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
454 
455  pclbuf_xyz->x = pclbuf_xyz->y = pclbuf_xyz->z = 0.f;
456  pcl_xyz.points[idx].x = pcl_xyz.points[idx].y = pcl_xyz.points[idx].z = 0.f;
457  } else {
458  // Fill in XYZ
459  pclbuf_rgb->x = pcl_rgb.points[idx].x = pclbuf_xyz->x = pcl_xyz.points[idx].x =
460  depth_data[idx] * 0.001f;
461  pclbuf_rgb->y = pcl_rgb.points[idx].y = pclbuf_xyz->y = pcl_xyz.points[idx].y =
462  -(w - __center_x) * depth_data[idx] * __foc_const;
463  pclbuf_rgb->z = pcl_rgb.points[idx].z = pclbuf_xyz->z = pcl_xyz.points[idx].z =
464  -(h - __center_y) * depth_data[idx] * __foc_const;
465  }
466  }
467  }
468 
469  fill_rgb(pcl_rgb);
470 
471  __pcl_xyzrgb_buf->unlock();
472  __pcl_xyz_buf->unlock();
473 }
474 
475 
476 void
477 OpenNiPointCloudThread::fill_rgb(pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb)
478 {
479  if (! __image_rgb_buf) {
480  try {
481  __image_rgb_buf = new SharedMemoryImageBuffer("openni-image-rgb");
482  } catch (Exception &e) {
483  logger->log_warn(name(), "Failed to open openni-image-rgb shm image buffer");
484  return;
485  }
486  }
487 
488  __img_thread->wait_loop_done();
489 
490  register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
491  register RGB_t *imagebuf = (RGB_t *)__image_rgb_buf->buffer();
492 
493  for (unsigned int i = 0; i < __width * __height; ++i) {
494  pclbuf_rgb->r = pcl_rgb.points[i].r = imagebuf[i].R;
495  pclbuf_rgb->g = pcl_rgb.points[i].g = imagebuf[i].G;
496  pclbuf_rgb->b = pcl_rgb.points[i].b = imagebuf[i].B;
497  }
498 }
499 
500 #endif
501 
502 
503 void
505 {
507  bool is_data_new = __depth_gen->IsDataNew();
508  __depth_gen->GetMetaData(*__depth_md);
509  const XnDepthPixel * const data = __depth_md->Data();
510  // experimental: unlock here as we do not invoke any methods anymore
511  // since data has been updated earlier in the sensor hook we should be safe
512  lock.unlock();
513 
514  bool xyz_requested = (__pcl_xyz_buf->num_attached() > 1)
515 #ifdef HAVE_PCL
516  // 2 is us and the PCL manager of the PointCloudAspect
517  || (__cfg_generate_pcl && ((__pcl_xyz.use_count() > 2)))
518 #endif
519  ;
520  bool xyzrgb_requested = (__pcl_xyzrgb_buf->num_attached() > 1)
521 #ifdef HAVE_PCL
522  // 2 is us and the PCL manager of the PointCloudAspect
523  || (__cfg_generate_pcl && ((__pcl_xyzrgb.use_count() > 2)))
524 #endif
525  ;
526 
527  if (is_data_new && (xyz_requested || xyzrgb_requested)) {
528  // convert depth to points
529  fawkes::Time ts = *__capture_start + (long int)__depth_gen->GetTimestamp();
530 
531 #ifdef HAVE_PCL
532  if (__cfg_generate_pcl) {
533 
534  if (xyz_requested && xyzrgb_requested) {
535  fill_xyz_xyzrgb(ts, data);
536  } else if (xyz_requested) {
537  fill_xyz(ts, data);
538  } else if (xyzrgb_requested) {
539  fill_xyzrgb(ts, data);
540  }
541 
542  } else {
543 #endif
544  if (xyz_requested && xyzrgb_requested) {
545  fill_xyz_xyzrgb_no_pcl(ts, data);
546  } else if (xyz_requested) {
547  fill_xyz_no_pcl(ts, data);
548  } else if (xyzrgb_requested) {
549  fill_xyzrgb_no_pcl(ts, data);
550  }
551 #ifdef HAVE_PCL
552  }
553 #endif
554 
555  // close rgb image buffer if no longer required
556  if (! xyzrgb_requested && __image_rgb_buf) {
557  delete __image_rgb_buf;
558  __image_rgb_buf = NULL;
559  }
560  }
561 }