oni_grabber.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2011-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  */
00036 
00037 #include <pcl/pcl_config.h>
00038 #ifdef HAVE_OPENNI
00039 
00040 #include <pcl/io/oni_grabber.h>
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/point_types.h>
00043 #include <pcl/common/time.h>
00044 #include <pcl/console/print.h>
00045 #include <pcl/io/pcl_io_exception.h>
00046 #include <boost/shared_array.hpp>
00047 #include <iostream>
00048 
00049 namespace pcl
00050 {
00051   typedef union
00052   {
00053     struct /*anonymous*/
00054     {
00055       unsigned char Blue;
00056       unsigned char Green;
00057       unsigned char Red;
00058       unsigned char Alpha;
00059     };
00060     float float_value;
00061     long long_value;
00062   } RGBValue;
00063 
00065 ONIGrabber::ONIGrabber (const std::string& file_name, bool repeat, bool stream)
00066   : rgb_sync_ ()
00067   , ir_sync_ ()
00068   , device_ ()
00069   , rgb_frame_id_ ("/openni_rgb_optical_frame")
00070   , depth_frame_id_ ("/openni_depth_optical_frame")
00071   , running_ (false)
00072   , image_width_ ()
00073   , image_height_ ()
00074   , depth_width_ ()
00075   , depth_height_ ()
00076   , depth_callback_handle ()
00077   , image_callback_handle ()
00078   , ir_callback_handle ()
00079   , image_signal_ (), depth_image_signal_ (), ir_image_signal_ (), image_depth_image_signal_ ()
00080   , ir_depth_image_signal_ (), point_cloud_signal_ (), point_cloud_i_signal_ (), point_cloud_rgb_signal_ ()
00081   , point_cloud_rgba_signal_ ()
00082 {
00083   openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00084   device_ = boost::dynamic_pointer_cast< openni_wrapper::DeviceONI> (driver.createVirtualDevice (file_name, repeat, stream));
00085 
00086   if (!device_->hasDepthStream ())
00087     THROW_PCL_IO_EXCEPTION("Device does not provide 3D information.");
00088 
00089   XnMapOutputMode depth_mode = device_->getDepthOutputMode();
00090   depth_width_ = depth_mode.nXRes;
00091   depth_height_ = depth_mode.nYRes;
00092 
00093   depth_image_signal_ = createSignal <sig_cb_openni_depth_image > ();
00094   point_cloud_signal_ = createSignal <sig_cb_openni_point_cloud > ();
00095 
00096   if (device_->hasIRStream ())
00097   {
00098     ir_image_signal_        = createSignal <sig_cb_openni_ir_image > ();
00099     point_cloud_i_signal_   = createSignal <sig_cb_openni_point_cloud_i > ();
00100     ir_depth_image_signal_  = createSignal <sig_cb_openni_ir_depth_image > ();
00101   }
00102 
00103   if (device_->hasImageStream ())
00104   {
00105     XnMapOutputMode depth_mode = device_->getImageOutputMode ();
00106     image_width_ = depth_mode.nXRes;
00107     image_height_ = depth_mode.nYRes;
00108 
00109     image_signal_             = createSignal <sig_cb_openni_image> ();
00110     image_depth_image_signal_ = createSignal <sig_cb_openni_image_depth_image> ();
00111     point_cloud_rgb_signal_   = createSignal <sig_cb_openni_point_cloud_rgb> ();
00112     point_cloud_rgba_signal_   = createSignal <sig_cb_openni_point_cloud_rgba> ();
00113     rgb_sync_.addCallback (boost::bind(&ONIGrabber::imageDepthImageCallback, this, _1, _2));
00114   }
00115 
00116   image_callback_handle = device_->registerImageCallback (&ONIGrabber::imageCallback, *this);
00117   depth_callback_handle = device_->registerDepthCallback (&ONIGrabber::depthCallback, *this);
00118   ir_callback_handle    = device_->registerIRCallback (&ONIGrabber::irCallback, *this);
00119 
00120   // if in trigger mode -> publish these topics
00121   if (!stream)
00122   {
00123     // check if we need to start/stop any stream
00124     if (device_->hasImageStream () && !device_->isImageStreamRunning ())
00125       device_->startImageStream ();
00126 
00127     if (device_->hasDepthStream () && !device_->isDepthStreamRunning ())
00128       device_->startDepthStream ();
00129 
00130     if (device_->hasIRStream () && !device_->isIRStreamRunning ())
00131       device_->startIRStream ();
00132   }
00133 }
00134 
00136 ONIGrabber::~ONIGrabber() throw ()
00137 {
00138   try
00139   {
00140     stop();
00141     // unregister callbacks
00142     device_->unregisterDepthCallback(depth_callback_handle);
00143     device_->unregisterImageCallback(image_callback_handle);
00144     device_->unregisterIRCallback(image_callback_handle);
00145 
00146     // disconnect all listeners
00147     disconnect_all_slots <sig_cb_openni_image> ();
00148     disconnect_all_slots <sig_cb_openni_depth_image> ();
00149     disconnect_all_slots <sig_cb_openni_ir_image> ();
00150     disconnect_all_slots <sig_cb_openni_image_depth_image> ();
00151     disconnect_all_slots <sig_cb_openni_point_cloud> ();
00152     disconnect_all_slots <sig_cb_openni_point_cloud_rgb> ();
00153     disconnect_all_slots <sig_cb_openni_point_cloud_rgba> ();
00154     disconnect_all_slots <sig_cb_openni_point_cloud_i > ();
00155   }
00156   catch (...)
00157   {
00158     // destructor never throws
00159   }
00160 }
00161 
00163 void 
00164 ONIGrabber::start ()
00165 {
00166   if (device_->isStreaming ())
00167   {
00168     try
00169     {
00170       // check if we need to start/stop any stream
00171       if (device_->hasImageStream() && !device_->isImageStreamRunning())
00172         device_->startImageStream();
00173 
00174       if (device_->hasDepthStream() && !device_->isDepthStreamRunning())
00175         device_->startDepthStream();
00176 
00177       if (device_->hasIRStream() && !device_->isIRStreamRunning())
00178         device_->startIRStream();
00179 
00180       running_ = true;
00181     }
00182     catch (openni_wrapper::OpenNIException& ex)
00183     {
00184       THROW_PCL_IO_EXCEPTION("Could not start streams. Reason: %s", ex.what());
00185     }
00186   }
00187   else
00188   {
00189     if (device_->hasImageStream ())
00190       device_->trigger ();
00191 
00192     if (device_->hasDepthStream ())
00193       device_->trigger ();
00194 
00195     if (device_->hasIRStream ())
00196       device_->trigger ();
00197   }
00198 }
00199 
00201 void 
00202 ONIGrabber::stop ()
00203 {
00204   if (device_->isStreaming ())
00205   {
00206     try
00207     {
00208       if (device_->hasDepthStream() && device_->isDepthStreamRunning())
00209         device_->stopDepthStream();
00210 
00211       if (device_->hasImageStream() && device_->isImageStreamRunning())
00212         device_->stopImageStream();
00213 
00214       if (device_->hasIRStream() && device_->isIRStreamRunning())
00215         device_->stopIRStream();
00216 
00217       running_ = false;
00218     }
00219     catch (openni_wrapper::OpenNIException& ex)
00220     {
00221       THROW_PCL_IO_EXCEPTION("Could not stop streams. Reason: %s", ex.what());
00222     }
00223   }
00224 }
00225 
00227 bool 
00228 ONIGrabber::isRunning() const
00229 {
00230   return (running_);
00231 }
00232 
00234 std::string 
00235 ONIGrabber::getName () const
00236 {
00237   return (std::string("ONIGrabber"));
00238 }
00239 
00241 float 
00242 ONIGrabber::getFramesPerSecond () const
00243 {
00244   if (device_->isStreaming())
00245     return (static_cast<float> (device_->getDepthOutputMode ().nFPS));
00246   else
00247     return (0);
00248 }
00249 
00251 void 
00252 ONIGrabber::imageCallback(boost::shared_ptr<openni_wrapper::Image> image, void*)
00253 {
00254   if (num_slots<sig_cb_openni_point_cloud_rgb> () > 0 ||
00255       num_slots<sig_cb_openni_point_cloud_rgba> () > 0 ||
00256       num_slots<sig_cb_openni_image_depth_image > () > 0)
00257     rgb_sync_.add0(image, image->getTimeStamp());
00258 
00259   if (image_signal_->num_slots() > 0)
00260     image_signal_->operator()(image);
00261 
00262   return;
00263 }
00264 
00266 void 
00267 ONIGrabber::depthCallback(boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void*)
00268 {
00269   if (num_slots<sig_cb_openni_point_cloud_rgb> () > 0 ||
00270       num_slots<sig_cb_openni_point_cloud_rgba> () > 0 ||
00271       num_slots<sig_cb_openni_image_depth_image> () > 0)
00272     rgb_sync_.add1(depth_image, depth_image->getTimeStamp());
00273 
00274   if (num_slots<sig_cb_openni_point_cloud_i > () > 0 ||
00275       num_slots<sig_cb_openni_ir_depth_image > () > 0)
00276     ir_sync_.add1(depth_image, depth_image->getTimeStamp());
00277 
00278   if (depth_image_signal_->num_slots() > 0)
00279     depth_image_signal_->operator()(depth_image);
00280 
00281   if (point_cloud_signal_->num_slots() > 0)
00282     point_cloud_signal_->operator()(convertToXYZPointCloud(depth_image));
00283 
00284   return;
00285 }
00286 
00288 void 
00289 ONIGrabber::irCallback(boost::shared_ptr<openni_wrapper::IRImage> ir_image, void*)
00290 {
00291   if (num_slots<sig_cb_openni_point_cloud_i > () > 0 ||
00292       num_slots<sig_cb_openni_ir_depth_image > () > 0)
00293     ir_sync_.add0(ir_image, ir_image->getTimeStamp());
00294 
00295   if (ir_image_signal_->num_slots() > 0)
00296     ir_image_signal_->operator()(ir_image);
00297 
00298   return;
00299 }
00300 
00302 void 
00303 ONIGrabber::imageDepthImageCallback(const boost::shared_ptr<openni_wrapper::Image> &image, const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image)
00304 {
00305   // check if we have color point cloud slots
00306   if (point_cloud_rgb_signal_->num_slots () > 0)
00307   {
00308     PCL_WARN ("PointXYZRGB callbacks deprecated. Use PointXYZRGBA instead.");
00309     point_cloud_rgb_signal_->operator() (convertToXYZRGBPointCloud (image, depth_image));
00310   }
00311 
00312   if (point_cloud_rgba_signal_->num_slots () > 0)
00313     point_cloud_rgba_signal_->operator() (convertToXYZRGBAPointCloud (image, depth_image));
00314 
00315   if (image_depth_image_signal_->num_slots() > 0)
00316   {
00317     float constant = 1.0f / device_->getDepthFocalLength(depth_width_);
00318     image_depth_image_signal_->operator()(image, depth_image, constant);
00319   }
00320 }
00321 
00323 void 
00324 ONIGrabber::irDepthImageCallback(const boost::shared_ptr<openni_wrapper::IRImage> &ir_image, const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image)
00325 {
00326   // check if we have color point cloud slots
00327   if (point_cloud_i_signal_->num_slots() > 0)
00328     point_cloud_i_signal_->operator()(convertToXYZIPointCloud(ir_image, depth_image));
00329 
00330   if (ir_depth_image_signal_->num_slots() > 0)
00331   {
00332     float constant = 1.0f / device_->getDepthFocalLength(depth_width_);
00333     ir_depth_image_signal_->operator()(ir_image, depth_image, constant);
00334   }
00335 }
00336 
00338 pcl::PointCloud<pcl::PointXYZ>::Ptr 
00339 ONIGrabber::convertToXYZPointCloud(const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image) const
00340 {
00341   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud <pcl::PointXYZ>);
00342 
00343   // TODO cloud->header.stamp = time;
00344   cloud->height = depth_height_;
00345   cloud->width = depth_width_;
00346   cloud->is_dense = false;
00347 
00348   cloud->points.resize (cloud->height * cloud->width);
00349 
00350   register float constant = 1.0f / device_->getDepthFocalLength (depth_width_);
00351 
00352   if (device_->isDepthRegistered ())
00353     cloud->header.frame_id = rgb_frame_id_;
00354   else
00355     cloud->header.frame_id = depth_frame_id_;
00356 
00357   register int centerX = (cloud->width >> 1);
00358   int centerY = (cloud->height >> 1);
00359 
00360   float bad_point = std::numeric_limits<float>::quiet_NaN ();
00361 
00362   // we have to use Data, since operator[] uses assert -> Debug-mode very slow!
00363   register const unsigned short* depth_map = depth_image->getDepthMetaData ().Data ();
00364   if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_)
00365   {
00366     static unsigned buffer_size = 0;
00367     static boost::shared_array<unsigned short> depth_buffer (0);
00368 
00369     if (buffer_size < depth_width_ * depth_height_)
00370     {
00371       buffer_size = depth_width_ * depth_height_;
00372       depth_buffer.reset (new unsigned short [buffer_size]);
00373     }
00374     depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ());
00375     depth_map = depth_buffer.get ();
00376   }
00377 
00378   register int depth_idx = 0;
00379   for (int v = -centerY; v < centerY; ++v)
00380   {
00381     for (register int u = -centerX; u < centerX; ++u, ++depth_idx)
00382     {
00383       pcl::PointXYZ& pt = cloud->points[depth_idx];
00384       // Check for invalid measurements
00385       if (depth_map[depth_idx] == 0 ||
00386           depth_map[depth_idx] == depth_image->getNoSampleValue () ||
00387           depth_map[depth_idx] == depth_image->getShadowValue ())
00388       {
00389         // not valid
00390         pt.x = pt.y = pt.z = bad_point;
00391         continue;
00392       }
00393       pt.z = depth_map[depth_idx] * 0.001f;
00394       pt.x = static_cast<float> (u) * pt.z * constant;
00395       pt.y = static_cast<float> (v) * pt.z * constant;
00396     }
00397   }
00398   return (cloud);
00399 }
00400 
00402 pcl::PointCloud<pcl::PointXYZRGB>::Ptr ONIGrabber::convertToXYZRGBPointCloud (
00403     const boost::shared_ptr<openni_wrapper::Image> &image, 
00404     const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
00405 {
00406   static unsigned rgb_array_size = 0;
00407   static boost::shared_array<unsigned char> rgb_array(0);
00408   static unsigned char* rgb_buffer = 0;
00409 
00410   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
00411 
00412   cloud->header.frame_id = rgb_frame_id_;
00413   cloud->height = depth_height_;
00414   cloud->width = depth_width_;
00415   cloud->is_dense = false;
00416 
00417   cloud->points.resize(cloud->height * cloud->width);
00418 
00419   float constant = 1.0f / device_->getImageFocalLength(cloud->width);
00420   register int centerX = (cloud->width >> 1);
00421   int centerY = (cloud->height >> 1);
00422 
00423   register const XnDepthPixel* depth_map = depth_image->getDepthMetaData().Data();
00424   if (depth_image->getWidth() != depth_width_ || depth_image->getHeight() != depth_height_)
00425   {
00426     static unsigned buffer_size = 0;
00427     static boost::shared_array<unsigned short> depth_buffer(0);
00428 
00429     if (buffer_size < depth_width_ * depth_height_)
00430     {
00431       buffer_size = depth_width_ * depth_height_;
00432       depth_buffer.reset (new unsigned short [buffer_size]);
00433     }
00434 
00435     depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get());
00436     depth_map = depth_buffer.get ();
00437   }
00438 
00439   // here we need exact the size of the point cloud for a one-one correspondence!
00440   if (rgb_array_size < image_width_ * image_height_ * 3)
00441   {
00442     rgb_array_size = image_width_ * image_height_ * 3;
00443     rgb_array.reset(new unsigned char [rgb_array_size]);
00444     rgb_buffer = rgb_array.get();
00445   }
00446   image->fillRGB(image_width_, image_height_, rgb_buffer, image_width_ * 3);
00447 
00448   // depth_image already has the desired dimensions, but rgb_msg may be higher res.
00449   register int color_idx = 0, depth_idx = 0;
00450   RGBValue color;
00451   color.Alpha = 0;
00452 
00453   float bad_point = std::numeric_limits<float>::quiet_NaN();
00454 
00455   for (int v = -centerY; v < centerY; ++v)
00456   {
00457     for (register int u = -centerX; u < centerX; ++u, color_idx += 3, ++depth_idx)
00458     {
00459       pcl::PointXYZRGB& pt = cloud->points[depth_idx];
00461       // Check for invalid measurements
00462       if (depth_map[depth_idx] == 0 ||
00463           depth_map[depth_idx] == depth_image->getNoSampleValue() ||
00464           depth_map[depth_idx] == depth_image->getShadowValue())
00465       {
00466         pt.x = pt.y = pt.z = bad_point;
00467       }
00468       else
00469       {
00470         pt.z = depth_map[depth_idx] * 0.001f;
00471         pt.x = static_cast<float> (u) * pt.z * constant;
00472         pt.y = static_cast<float> (v) * pt.z * constant;
00473       }
00474 
00475       // Fill in color
00476       color.Red = rgb_buffer[color_idx];
00477       color.Green = rgb_buffer[color_idx + 1];
00478       color.Blue = rgb_buffer[color_idx + 2];
00479       pt.rgb = color.float_value;
00480     }
00481   }
00482   return (cloud);
00483 }
00484 
00486 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ONIGrabber::convertToXYZRGBAPointCloud (
00487     const boost::shared_ptr<openni_wrapper::Image> &image, 
00488     const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
00489 {
00490   static unsigned rgb_array_size = 0;
00491   static boost::shared_array<unsigned char> rgb_array(0);
00492   static unsigned char* rgb_buffer = 0;
00493 
00494   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > cloud (new pcl::PointCloud<pcl::PointXYZRGBA> ());
00495 
00496   cloud->header.frame_id = rgb_frame_id_;
00497   cloud->height = depth_height_;
00498   cloud->width = depth_width_;
00499   cloud->is_dense = false;
00500 
00501   cloud->points.resize(cloud->height * cloud->width);
00502 
00503   float constant = 1.0f / device_->getImageFocalLength(cloud->width);
00504   register int centerX = (cloud->width >> 1);
00505   int centerY = (cloud->height >> 1);
00506 
00507   register const XnDepthPixel* depth_map = depth_image->getDepthMetaData().Data();
00508   if (depth_image->getWidth() != depth_width_ || depth_image->getHeight() != depth_height_)
00509   {
00510     static unsigned buffer_size = 0;
00511     static boost::shared_array<unsigned short> depth_buffer(0);
00512 
00513     if (buffer_size < depth_width_ * depth_height_)
00514     {
00515       buffer_size = depth_width_ * depth_height_;
00516       depth_buffer.reset(new unsigned short [buffer_size]);
00517     }
00518 
00519     depth_image->fillDepthImageRaw(depth_width_, depth_height_, depth_buffer.get());
00520     depth_map = depth_buffer.get();
00521   }
00522 
00523   // here we need exact the size of the point cloud for a one-one correspondence!
00524   if (rgb_array_size < image_width_ * image_height_ * 3)
00525   {
00526     rgb_array_size = image_width_ * image_height_ * 3;
00527     rgb_array.reset (new unsigned char [rgb_array_size]);
00528     rgb_buffer = rgb_array.get();
00529   }
00530   image->fillRGB(image_width_, image_height_, rgb_buffer, image_width_ * 3);
00531 
00532   // depth_image already has the desired dimensions, but rgb_msg may be higher res.
00533   register int color_idx = 0, depth_idx = 0;
00534   RGBValue color;
00535   color.Alpha = 0;
00536 
00537   float bad_point = std::numeric_limits<float>::quiet_NaN();
00538 
00539   for (int v = -centerY; v < centerY; ++v)
00540   {
00541     for (register int u = -centerX; u < centerX; ++u, color_idx += 3, ++depth_idx)
00542     {
00543       pcl::PointXYZRGBA& pt = cloud->points[depth_idx];
00545       // Check for invalid measurements
00546       if (depth_map[depth_idx] == 0 ||
00547           depth_map[depth_idx] == depth_image->getNoSampleValue() ||
00548           depth_map[depth_idx] == depth_image->getShadowValue())
00549       {
00550         pt.x = pt.y = pt.z = bad_point;
00551       }
00552       else
00553       {
00554         pt.z = depth_map[depth_idx] * 0.001f;
00555         pt.x = static_cast<float> (u) * pt.z * constant;
00556         pt.y = static_cast<float> (v) * pt.z * constant;
00557       }
00558 
00559       // Fill in color
00560       color.Red = rgb_buffer[color_idx];
00561       color.Green = rgb_buffer[color_idx + 1];
00562       color.Blue = rgb_buffer[color_idx + 2];
00563       pt.rgba = static_cast<uint32_t> (color.long_value);
00564     }
00565   }
00566   return (cloud);
00567 }
00568 
00570 pcl::PointCloud<pcl::PointXYZI>::Ptr ONIGrabber::convertToXYZIPointCloud(const boost::shared_ptr<openni_wrapper::IRImage> &ir_image,
00571   const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
00572 {
00573   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > cloud(new pcl::PointCloud<pcl::PointXYZI > ());
00574 
00575   cloud->header.frame_id = rgb_frame_id_;
00576   cloud->height = depth_height_;
00577   cloud->width = depth_width_;
00578   cloud->is_dense = false;
00579 
00580   cloud->points.resize(cloud->height * cloud->width);
00581 
00582   float constant = 1.0f / device_->getImageFocalLength(cloud->width);
00583   register int centerX = (cloud->width >> 1);
00584   int centerY = (cloud->height >> 1);
00585 
00586   register const XnDepthPixel* depth_map = depth_image->getDepthMetaData().Data();
00587   register const XnIRPixel* ir_map = ir_image->getMetaData().Data();
00588 
00589   if (depth_image->getWidth() != depth_width_ || depth_image->getHeight() != depth_height_)
00590   {
00591     static unsigned buffer_size = 0;
00592     static boost::shared_array<unsigned short> depth_buffer(0);
00593     static boost::shared_array<unsigned short> ir_buffer(0);
00594 
00595     if (buffer_size < depth_width_ * depth_height_)
00596     {
00597       buffer_size = depth_width_ * depth_height_;
00598       depth_buffer.reset(new unsigned short [buffer_size]);
00599       ir_buffer.reset(new unsigned short [buffer_size]);
00600     }
00601 
00602     depth_image->fillDepthImageRaw(depth_width_, depth_height_, depth_buffer.get());
00603     depth_map = depth_buffer.get();
00604 
00605     ir_image->fillRaw(depth_width_, depth_height_, ir_buffer.get());
00606     ir_map = ir_buffer.get ();
00607   }
00608 
00609   register int depth_idx = 0;
00610   float bad_point = std::numeric_limits<float>::quiet_NaN();
00611 
00612   for (int v = -centerY; v < centerY; ++v)
00613   {
00614     for (register int u = -centerX; u < centerX; ++u, ++depth_idx)
00615     {
00616       pcl::PointXYZI& pt = cloud->points[depth_idx];
00618       // Check for invalid measurements
00619       if (depth_map[depth_idx] == 0 ||
00620           depth_map[depth_idx] == depth_image->getNoSampleValue() ||
00621           depth_map[depth_idx] == depth_image->getShadowValue())
00622       {
00623         pt.x = pt.y = pt.z = bad_point;
00624       }
00625       else
00626       {
00627         pt.z = depth_map[depth_idx] * 0.001f;
00628         pt.x = static_cast<float> (u) * pt.z * constant;
00629         pt.y = static_cast<float> (v) * pt.z * constant;
00630       }
00631 
00632       pt.data_c[0] = pt.data_c[1] = pt.data_c[2] = pt.data_c[3] = 0;
00633       pt.intensity = static_cast<float> (ir_map[depth_idx]);
00634     }
00635   }
00636   return (cloud);
00637 }
00638 
00639 }
00640 #endif // HAVE_OPENNI
00641 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:58