Fawkes API
Fawkes Development Version
|
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 }