Fawkes API  Fawkes Development Version
pointcloud_thread.cpp
00001 
00002 /***************************************************************************
00003  *  pointcloud_thread.cpp - OpenNI point cloud provider thread
00004  *
00005  *  Created: Fri Mar 25 23:49:11 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "pointcloud_thread.h"
00024 #include "image_thread.h"
00025 #include "utils/setup.h"
00026 
00027 #include <core/threading/mutex_locker.h>
00028 #include <fvutils/ipc/shm_image.h>
00029 #include <fvutils/color/colorspaces.h>
00030 #include <fvutils/base/types.h>
00031 #include <fvutils/color/rgb.h>
00032 #ifdef HAVE_PCL
00033 #  include <pcl_utils/utils.h>
00034 #endif
00035 
00036 #include <memory>
00037 
00038 using namespace fawkes;
00039 using namespace firevision;
00040 
00041 #define FRAME_ID_DEPTH "/kinect/depth"
00042 #define FRAME_ID_IMAGE "/kinect/image"
00043 
00044 /** @class OpenNiPointCloudThread "pointcloud_thread.h"
00045  * OpenNI Point Cloud Provider Thread.
00046  * This thread provides a point cloud calculated from the depth image
00047  * acquired via OpenNI and provides it as a
00048  * SharedMemoryImageBuffer to other FireVision plugins.
00049  *
00050  * @author Tim Niemueller
00051  */
00052 
00053 /** Constructor.
00054  * @param img_thread OpenNI image thread, used for XYZRGB point clouds
00055  */
00056 OpenNiPointCloudThread::OpenNiPointCloudThread(OpenNiImageThread *img_thread)
00057   : Thread("OpenNiPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
00058     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
00059 {
00060   __img_thread = img_thread;
00061 }
00062 
00063 
00064 /** Destructor. */
00065 OpenNiPointCloudThread::~OpenNiPointCloudThread()
00066 {
00067 }
00068 
00069 
00070 void
00071 OpenNiPointCloudThread::init()
00072 {
00073   MutexLocker lock(openni.objmutex_ptr());
00074 
00075   __image_rgb_buf = NULL;
00076 
00077   __depth_gen = new xn::DepthGenerator();
00078   std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
00079   __image_gen = new xn::ImageGenerator();
00080   std::auto_ptr<xn::ImageGenerator> imagegen_autoptr(__image_gen);
00081 
00082   XnStatus st;
00083 
00084   fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, __depth_gen);
00085   fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_IMAGE, __image_gen);
00086   fawkes::openni::setup_map_generator(*__image_gen, config);
00087   fawkes::openni::setup_map_generator(*__depth_gen, config);
00088 
00089   __depth_md = new xn::DepthMetaData();
00090   __depth_gen->GetMetaData(*__depth_md);
00091 
00092   __cfg_register_depth_image = false;
00093   try {
00094     __cfg_register_depth_image = config->get_bool("/plugins/openni/register_depth_image");
00095   } catch (Exception &e) {}
00096 
00097   __pcl_xyz_buf = new SharedMemoryImageBuffer("openni-pointcloud-xyz",
00098                                           CARTESIAN_3D_FLOAT,
00099                                           __depth_md->XRes(), __depth_md->YRes());
00100 
00101   __pcl_xyz_buf->set_frame_id(__cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH);
00102 
00103 
00104   __pcl_xyzrgb_buf = new SharedMemoryImageBuffer("openni-pointcloud-xyzrgb",
00105                                                  CARTESIAN_3D_FLOAT_RGB,
00106                                                  __depth_md->XRes(), __depth_md->YRes());
00107 
00108   __pcl_xyzrgb_buf->set_frame_id(__cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH);
00109 
00110   // this is magic from ROS openni_device.cpp, reading code from
00111   // openni-primesense suggests that SXGA is the base configuration
00112   XnUInt64 zpd; // zero plane distance
00113   if ((st = __depth_gen->GetIntProperty("ZPD", zpd)) != XN_STATUS_OK) {
00114     throw Exception("Failed to get ZPD: %s", xnGetStatusString(st));
00115   }
00116   XnDouble pixel_size; // zero plane pixel size
00117   if ((st = __depth_gen->GetRealProperty("ZPPS", pixel_size)) != XN_STATUS_OK) {
00118     throw Exception("Failed to get ZPPS: %s", xnGetStatusString(st));
00119   }
00120 
00121   if ((st = __depth_gen->GetIntProperty("NoSampleValue", __no_sample_value))
00122       != XN_STATUS_OK)
00123   {
00124     throw Exception("Failed to get NoSampleValue: %s", xnGetStatusString(st));
00125   }
00126   if ((st = __depth_gen->GetIntProperty("ShadowValue", __shadow_value))
00127       != XN_STATUS_OK)
00128   {
00129     throw Exception("Failed to get ShadowValue: %s", xnGetStatusString(st));
00130   }
00131 
00132   __width  = __depth_md->XRes();
00133   __height = __depth_md->YRes();
00134   float scale = __width / (float)XN_SXGA_X_RES;
00135   if (__cfg_register_depth_image) {
00136     // magic number taken from ROS/PCL openni_device.cpp
00137     const float rgb_focal_length_SXGA = 1050;
00138     __focal_length = rgb_focal_length_SXGA * scale;
00139   } else {
00140     __focal_length = ((float)zpd / pixel_size) * scale;
00141   }
00142   __foc_const = 0.001 / __focal_length;
00143   __center_x = (__width  / 2.) - .5f;
00144   __center_y = (__height / 2.) - .5f;
00145 
00146   __image_gen->StartGenerating();
00147   __depth_gen->StartGenerating();
00148 
00149   __capture_start = new Time(clock);
00150   __capture_start->stamp_systime();
00151   // Update once to get timestamp
00152   __depth_gen->WaitAndUpdateData();
00153   // arbitrarily define the zero reference point,
00154   // we can't get any closer than this
00155   *__capture_start -= (long int)__depth_gen->GetTimestamp();
00156 
00157   __image_gen->WaitAndUpdateData();
00158 
00159   if (__cfg_register_depth_image) {
00160     logger->log_info(name(), "Setting depth alternate viewpoint to image");
00161     fawkes::openni::setup_alternate_viewpoint(*__depth_gen, *__image_gen);
00162   }
00163 
00164   // Fails with "Bad Paramter" on OpenNI 1.3.2.1/PS 5.0.3.3
00165   //logger->log_info(name(), "Setting depth/image synchronization");
00166   //fawkes::openni::setup_synchronization(*__depth_gen, *__image_gen);
00167 
00168 #ifdef HAVE_PCL
00169   __cfg_generate_pcl = true;
00170   try {
00171     __cfg_generate_pcl = config->get_bool("/plugins/openni-pointcloud/generate-pcl");
00172   } catch (Exception &e) {}
00173 
00174   if (__cfg_generate_pcl) {
00175     __pcl_xyz = new pcl::PointCloud<pcl::PointXYZ>();
00176     __pcl_xyz->is_dense = false;
00177     __pcl_xyz->width    = __width;
00178     __pcl_xyz->height   = __height;
00179     __pcl_xyz->points.resize(__width * __height);
00180     __pcl_xyz->header.frame_id = __cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH;
00181 
00182     __pcl_xyzrgb = new pcl::PointCloud<pcl::PointXYZRGB>();
00183     __pcl_xyzrgb->is_dense = false;
00184     __pcl_xyzrgb->width    = __width;
00185     __pcl_xyzrgb->height   = __height;
00186     __pcl_xyzrgb->points.resize(__width * __height);
00187     __pcl_xyzrgb->header.frame_id = __cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH;
00188 
00189     pcl_manager->add_pointcloud("openni-pointcloud-xyz", __pcl_xyz);
00190     pcl_manager->add_pointcloud("openni-pointcloud-xyzrgb", __pcl_xyzrgb);
00191   }
00192 #endif
00193 
00194   depthgen_autoptr.release();
00195   imagegen_autoptr.release();
00196 }
00197 
00198 
00199 void
00200 OpenNiPointCloudThread::finalize()
00201 {
00202 #ifdef HAVE_PCL
00203   pcl_manager->remove_pointcloud("openni-pointcloud-xyz");
00204   pcl_manager->remove_pointcloud("openni-pointcloud-xyzrgb");
00205 #endif
00206 
00207   // we do not stop generating, we don't know if there is no other plugin
00208   // using the node.
00209   delete __depth_gen;
00210   delete __depth_md;
00211   delete __pcl_xyz_buf;
00212   delete __pcl_xyzrgb_buf;
00213   delete __capture_start;
00214 }
00215 
00216 
00217 void
00218 OpenNiPointCloudThread::fill_xyz_no_pcl(fawkes::Time &ts, const XnDepthPixel * const depth_data)
00219 {
00220   __pcl_xyz_buf->lock_for_write();
00221   __pcl_xyz_buf->set_capture_time(&ts);
00222 
00223   register pcl_point_t *pclbuf = (pcl_point_t *)__pcl_xyz_buf->buffer();
00224 
00225   unsigned int idx = 0;
00226   for (unsigned int h = 0; h < __height; ++h) {
00227     for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
00228       // Check for invalid measurements
00229       if (depth_data[idx] == 0 ||
00230           depth_data[idx] == __no_sample_value ||
00231           depth_data[idx] == __shadow_value)
00232       {
00233         // invalid
00234         pclbuf->x = pclbuf->y = pclbuf->z = 0.f;
00235       } else {
00236         // Fill in XYZ
00237         pclbuf->x = depth_data[idx] * 0.001f;
00238         pclbuf->y = -(w - __center_x) * depth_data[idx] * __foc_const;
00239         pclbuf->z = -(h - __center_y) * depth_data[idx] * __foc_const;
00240       }
00241     }
00242   }
00243 
00244   __pcl_xyz_buf->unlock();
00245 }
00246 
00247 
00248 void
00249 OpenNiPointCloudThread::fill_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const depth_data)
00250 {
00251   __pcl_xyzrgb_buf->lock_for_write();
00252   __pcl_xyzrgb_buf->set_capture_time(&ts);
00253 
00254   register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
00255 
00256   unsigned int idx = 0;
00257   for (unsigned int h = 0; h < __height; ++h) {
00258     for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
00259       // Check for invalid measurements
00260       if (depth_data[idx] == 0 ||
00261           depth_data[idx] == __no_sample_value ||
00262           depth_data[idx] == __shadow_value)
00263       {
00264         // invalid
00265         pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
00266       } else {
00267         // Fill in XYZ
00268         pclbuf_rgb->x = depth_data[idx] * 0.001f;
00269         pclbuf_rgb->y = -(w - __center_x) * depth_data[idx] * __foc_const;
00270         pclbuf_rgb->z = -(h - __center_y) * depth_data[idx] * __foc_const;
00271       }
00272     }
00273   }
00274 
00275   fill_rgb_no_pcl();
00276 
00277   __pcl_xyzrgb_buf->unlock();
00278 }
00279 
00280 
00281 void
00282 OpenNiPointCloudThread::fill_xyz_xyzrgb_no_pcl(fawkes::Time &ts,
00283                                                const XnDepthPixel * const depth_data)
00284 {
00285   __pcl_xyz_buf->lock_for_write();
00286   __pcl_xyz_buf->set_capture_time(&ts);
00287 
00288   __pcl_xyzrgb_buf->lock_for_write();
00289   __pcl_xyzrgb_buf->set_capture_time(&ts);
00290 
00291   register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
00292   register pcl_point_t *pclbuf_xyz = (pcl_point_t *)__pcl_xyz_buf->buffer();
00293 
00294   unsigned int idx = 0;
00295   for (unsigned int h = 0; h < __height; ++h) {
00296     for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
00297       // Check for invalid measurements
00298       if (depth_data[idx] == 0 ||
00299           depth_data[idx] == __no_sample_value ||
00300           depth_data[idx] == __shadow_value)
00301       {
00302         // invalid
00303         pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
00304         pclbuf_xyz->x = pclbuf_xyz->y = pclbuf_xyz->z = 0.f;
00305       } else {
00306         // Fill in XYZ
00307         pclbuf_rgb->x = pclbuf_xyz->x = depth_data[idx] * 0.001f;
00308         pclbuf_rgb->y = pclbuf_xyz->y = -(w - __center_x) * depth_data[idx] * __foc_const;
00309         pclbuf_rgb->z = pclbuf_xyz->z = -(h - __center_y) * depth_data[idx] * __foc_const;
00310       }
00311     }
00312   }
00313 
00314   fill_rgb_no_pcl();
00315 
00316   __pcl_xyzrgb_buf->unlock();
00317   __pcl_xyz_buf->unlock();
00318 }
00319 
00320 
00321 void
00322 OpenNiPointCloudThread::fill_rgb_no_pcl()
00323 {
00324   if (! __image_rgb_buf) {
00325     try {
00326       __image_rgb_buf = new SharedMemoryImageBuffer("openni-image-rgb");
00327     } catch (Exception &e) {
00328       logger->log_warn(name(), "Failed to open openni-image-rgb shm image buffer");
00329       return;
00330     }
00331   }
00332 
00333   __img_thread->wait_loop_done();
00334 
00335   register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
00336   register RGB_t *imagebuf = (RGB_t *)__image_rgb_buf->buffer();
00337 
00338   for (unsigned int i = 0; i < __width * __height; ++i) {
00339     pclbuf_rgb->r = imagebuf[i].R;
00340     pclbuf_rgb->g = imagebuf[i].G;
00341     pclbuf_rgb->b = imagebuf[i].B;
00342   }
00343 }
00344 
00345 
00346 
00347 #ifdef HAVE_PCL
00348 void
00349 OpenNiPointCloudThread::fill_xyz(fawkes::Time &ts, const XnDepthPixel * const depth_data)
00350 {
00351   pcl::PointCloud<pcl::PointXYZ> &pcl = **__pcl_xyz;
00352   pcl.header.seq += 1;
00353   pcl_utils::set_time(__pcl_xyz, ts);
00354 
00355   __pcl_xyz_buf->lock_for_write();
00356   __pcl_xyz_buf->set_capture_time(&ts);
00357 
00358   register pcl_point_t *pclbuf = (pcl_point_t *)__pcl_xyz_buf->buffer();
00359 
00360   unsigned int idx = 0;
00361   for (unsigned int h = 0; h < __height; ++h) {
00362     for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
00363       // Check for invalid measurements
00364       if (depth_data[idx] == 0 ||
00365           depth_data[idx] == __no_sample_value ||
00366           depth_data[idx] == __shadow_value)
00367       {
00368         // invalid
00369         pclbuf->x = pclbuf->y = pclbuf->z = 0.f;
00370         pcl.points[idx].x = pcl.points[idx].y = pcl.points[idx].z = 0.f;
00371       } else {
00372         // Fill in XYZ
00373         pclbuf->x = pcl.points[idx].x = depth_data[idx] * 0.001f;
00374         pclbuf->y = pcl.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
00375         pclbuf->z = pcl.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
00376       }
00377     }
00378   }
00379 
00380   __pcl_xyz_buf->unlock();
00381 }
00382 
00383 
00384 void
00385 OpenNiPointCloudThread::fill_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data)
00386 {
00387   pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb = **__pcl_xyzrgb;
00388   pcl_rgb.header.seq += 1;
00389   pcl_utils::set_time(__pcl_xyzrgb, ts);
00390 
00391   __pcl_xyzrgb_buf->lock_for_write();
00392   __pcl_xyzrgb_buf->set_capture_time(&ts);
00393 
00394   register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
00395 
00396   unsigned int idx = 0;
00397   for (unsigned int h = 0; h < __height; ++h) {
00398     for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
00399       // Check for invalid measurements
00400       if (depth_data[idx] == 0 ||
00401           depth_data[idx] == __no_sample_value ||
00402           depth_data[idx] == __shadow_value)
00403       {
00404         // invalid
00405         pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
00406         pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
00407       } else {
00408         // Fill in XYZ
00409         pclbuf_rgb->x = pcl_rgb.points[idx].x = depth_data[idx] * 0.001f;
00410         pclbuf_rgb->y = pcl_rgb.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
00411         pclbuf_rgb->z = pcl_rgb.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
00412       }
00413     }
00414   }
00415 
00416   fill_rgb(pcl_rgb);
00417 
00418   __pcl_xyzrgb_buf->unlock();
00419 }
00420 
00421 
00422 void
00423 OpenNiPointCloudThread::fill_xyz_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data)
00424 {
00425 
00426   pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb = **__pcl_xyzrgb;
00427   pcl_rgb.header.seq += 1;
00428   pcl_utils::set_time(__pcl_xyzrgb, ts);
00429 
00430   pcl::PointCloud<pcl::PointXYZ> &pcl_xyz = **__pcl_xyz;
00431   pcl_xyz.header.seq += 1;
00432   pcl_utils::set_time(__pcl_xyz, ts);
00433 
00434   __pcl_xyz_buf->lock_for_write();
00435   __pcl_xyz_buf->set_capture_time(&ts);
00436 
00437   __pcl_xyzrgb_buf->lock_for_write();
00438   __pcl_xyzrgb_buf->set_capture_time(&ts);
00439 
00440   register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
00441   register pcl_point_t *pclbuf_xyz = (pcl_point_t *)__pcl_xyz_buf->buffer();
00442 
00443   unsigned int idx = 0;
00444   for (unsigned int h = 0; h < __height; ++h) {
00445     for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
00446       // Check for invalid measurements
00447       if (depth_data[idx] == 0 ||
00448           depth_data[idx] == __no_sample_value ||
00449           depth_data[idx] == __shadow_value)
00450       {
00451         // invalid
00452         pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
00453         pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
00454 
00455         pclbuf_xyz->x = pclbuf_xyz->y = pclbuf_xyz->z = 0.f;
00456         pcl_xyz.points[idx].x = pcl_xyz.points[idx].y = pcl_xyz.points[idx].z = 0.f;
00457       } else {
00458         // Fill in XYZ
00459         pclbuf_rgb->x = pcl_rgb.points[idx].x = pclbuf_xyz->x = pcl_xyz.points[idx].x =
00460           depth_data[idx] * 0.001f;
00461         pclbuf_rgb->y = pcl_rgb.points[idx].y = pclbuf_xyz->y = pcl_xyz.points[idx].y =
00462           -(w - __center_x) * depth_data[idx] * __foc_const;
00463         pclbuf_rgb->z = pcl_rgb.points[idx].z = pclbuf_xyz->z = pcl_xyz.points[idx].z =
00464           -(h - __center_y) * depth_data[idx] * __foc_const;
00465       }
00466     }
00467   }
00468 
00469   fill_rgb(pcl_rgb);
00470 
00471   __pcl_xyzrgb_buf->unlock();
00472   __pcl_xyz_buf->unlock();
00473 }
00474 
00475 
00476 void
00477 OpenNiPointCloudThread::fill_rgb(pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb)
00478 {
00479   if (! __image_rgb_buf) {
00480     try {
00481       __image_rgb_buf = new SharedMemoryImageBuffer("openni-image-rgb");
00482     } catch (Exception &e) {
00483       logger->log_warn(name(), "Failed to open openni-image-rgb shm image buffer");
00484       return;
00485     }
00486   }
00487 
00488   __img_thread->wait_loop_done();
00489 
00490   register pcl_point_xyzrgb_t *pclbuf_rgb = (pcl_point_xyzrgb_t *)__pcl_xyzrgb_buf->buffer();
00491   register RGB_t *imagebuf = (RGB_t *)__image_rgb_buf->buffer();
00492 
00493   for (unsigned int i = 0; i < __width * __height; ++i) {
00494     pclbuf_rgb->r = pcl_rgb.points[i].r = imagebuf[i].R;
00495     pclbuf_rgb->g = pcl_rgb.points[i].g = imagebuf[i].G;
00496     pclbuf_rgb->b = pcl_rgb.points[i].b = imagebuf[i].B;
00497   }
00498 }
00499 
00500 #endif
00501 
00502 
00503 void
00504 OpenNiPointCloudThread::loop()
00505 {
00506   MutexLocker lock(openni.objmutex_ptr());
00507   bool is_data_new = __depth_gen->IsDataNew();
00508   __depth_gen->GetMetaData(*__depth_md);
00509   const XnDepthPixel * const data = __depth_md->Data();
00510   // experimental: unlock here as we do not invoke any methods anymore
00511   // since data has been updated earlier in the sensor hook we should be safe
00512   lock.unlock();
00513 
00514   bool xyz_requested = (__pcl_xyz_buf->num_attached() > 1)
00515 #ifdef HAVE_PCL
00516     // 2 is us and the PCL manager of the PointCloudAspect
00517     || (__cfg_generate_pcl && ((__pcl_xyz.use_count() > 2)))
00518 #endif
00519     ;
00520   bool xyzrgb_requested = (__pcl_xyzrgb_buf->num_attached() > 1)
00521 #ifdef HAVE_PCL
00522     // 2 is us and the PCL manager of the PointCloudAspect
00523     || (__cfg_generate_pcl && ((__pcl_xyzrgb.use_count() > 2)))
00524 #endif
00525     ;
00526 
00527   if (is_data_new && (xyz_requested || xyzrgb_requested)) {
00528     // convert depth to points
00529     fawkes::Time ts = *__capture_start + (long int)__depth_gen->GetTimestamp();
00530   
00531 #ifdef HAVE_PCL
00532     if (__cfg_generate_pcl) {
00533 
00534       if (xyz_requested && xyzrgb_requested) {
00535         fill_xyz_xyzrgb(ts, data);
00536       } else if (xyz_requested) {
00537         fill_xyz(ts, data);
00538       } else if (xyzrgb_requested) {
00539         fill_xyzrgb(ts, data);
00540       }
00541 
00542     } else {
00543 #endif
00544       if (xyz_requested && xyzrgb_requested) {
00545         fill_xyz_xyzrgb_no_pcl(ts, data);
00546       } else if (xyz_requested) {
00547         fill_xyz_no_pcl(ts, data);
00548       } else if (xyzrgb_requested) {
00549         fill_xyzrgb_no_pcl(ts, data);
00550       }
00551 #ifdef HAVE_PCL
00552     }
00553 #endif
00554 
00555     // close rgb image buffer if no longer required
00556     if (! xyzrgb_requested && __image_rgb_buf) {
00557       delete __image_rgb_buf;
00558       __image_rgb_buf = NULL;
00559     }
00560   }
00561 }