Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2009-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #include <pcl/pcl_config.h> 00040 #ifdef HAVE_OPENNI 00041 00042 #ifndef __PCL_IO_OPENNI_GRABBER__ 00043 #define __PCL_IO_OPENNI_GRABBER__ 00044 00045 #include <pcl/io/eigen.h> 00046 #include <pcl/io/boost.h> 00047 #include <pcl/io/grabber.h> 00048 #include <pcl/io/openni_camera/openni_driver.h> 00049 #include <pcl/io/openni_camera/openni_device_kinect.h> 00050 #include <pcl/io/openni_camera/openni_image.h> 00051 #include <pcl/io/openni_camera/openni_depth_image.h> 00052 #include <pcl/io/openni_camera/openni_ir_image.h> 00053 #include <string> 00054 #include <deque> 00055 #include <pcl/common/synchronizer.h> 00056 00057 namespace pcl 00058 { 00059 struct PointXYZ; 00060 struct PointXYZRGB; 00061 struct PointXYZRGBA; 00062 struct PointXYZI; 00063 template <typename T> class PointCloud; 00064 00065 /** \brief Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) 00066 * \author Nico Blodow <blodow@cs.tum.edu>, Suat Gedikli <gedikli@willowgarage.com> 00067 * \ingroup io 00068 */ 00069 class PCL_EXPORTS OpenNIGrabber : public Grabber 00070 { 00071 public: 00072 typedef boost::shared_ptr<OpenNIGrabber> Ptr; 00073 typedef boost::shared_ptr<const OpenNIGrabber> ConstPtr; 00074 00075 typedef enum 00076 { 00077 OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz 00078 OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect 00079 OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect 00080 OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion 00081 OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion 00082 OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect 00083 OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion 00084 OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN) 00085 OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN) 00086 OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN) 00087 } Mode; 00088 00089 //define callback signature typedefs 00090 typedef void (sig_cb_openni_image) (const boost::shared_ptr<openni_wrapper::Image>&); 00091 typedef void (sig_cb_openni_depth_image) (const boost::shared_ptr<openni_wrapper::DepthImage>&); 00092 typedef void (sig_cb_openni_ir_image) (const boost::shared_ptr<openni_wrapper::IRImage>&); 00093 typedef void (sig_cb_openni_image_depth_image) (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ; 00094 typedef void (sig_cb_openni_ir_depth_image) (const boost::shared_ptr<openni_wrapper::IRImage>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ; 00095 typedef void (sig_cb_openni_point_cloud) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&); 00096 typedef void (sig_cb_openni_point_cloud_rgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB> >&); 00097 typedef void (sig_cb_openni_point_cloud_rgba) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&); 00098 typedef void (sig_cb_openni_point_cloud_i) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&); 00099 00100 public: 00101 /** \brief Constructor 00102 * \param[in] device_id ID of the device, which might be a serial number, bus@address or the index of the device. 00103 * \param[in] depth_mode the mode of the depth stream 00104 * \param[in] image_mode the mode of the image stream 00105 */ 00106 OpenNIGrabber (const std::string& device_id = "", 00107 const Mode& depth_mode = OpenNI_Default_Mode, 00108 const Mode& image_mode = OpenNI_Default_Mode); 00109 00110 /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ 00111 virtual ~OpenNIGrabber () throw (); 00112 00113 /** \brief Start the data acquisition. */ 00114 virtual void 00115 start (); 00116 00117 /** \brief Stop the data acquisition. */ 00118 virtual void 00119 stop (); 00120 00121 /** \brief Check if the data acquisition is still running. */ 00122 virtual bool 00123 isRunning () const; 00124 00125 virtual std::string 00126 getName () const; 00127 00128 /** \brief Obtain the number of frames per second (FPS). */ 00129 virtual float 00130 getFramesPerSecond () const; 00131 00132 /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */ 00133 inline boost::shared_ptr<openni_wrapper::OpenNIDevice> 00134 getDevice () const; 00135 00136 /** \brief Obtain a list of the available depth modes that this device supports. */ 00137 std::vector<std::pair<int, XnMapOutputMode> > 00138 getAvailableDepthModes () const; 00139 00140 /** \brief Obtain a list of the available image modes that this device supports. */ 00141 std::vector<std::pair<int, XnMapOutputMode> > 00142 getAvailableImageModes () const; 00143 00144 /** \brief Set the RGB camera parameters (fx, fy, cx, cy) 00145 * \param[in] rgb_focal_length_x the RGB focal length (fx) 00146 * \param[in] rgb_focal_length_y the RGB focal length (fy) 00147 * \param[in] rgb_principal_point_x the RGB principal point (cx) 00148 * \param[in] rgb_principal_point_y the RGB principal point (cy) 00149 * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them 00150 * and the grabber will use the default values from the camera instead. 00151 */ 00152 inline void 00153 setRGBCameraIntrinsics (const double rgb_focal_length_x, 00154 const double rgb_focal_length_y, 00155 const double rgb_principal_point_x, 00156 const double rgb_principal_point_y) 00157 { 00158 rgb_focal_length_x_ = rgb_focal_length_x; 00159 rgb_focal_length_y_ = rgb_focal_length_y; 00160 rgb_principal_point_x_ = rgb_principal_point_x; 00161 rgb_principal_point_y_ = rgb_principal_point_y; 00162 } 00163 00164 /** \brief Get the RGB camera parameters (fx, fy, cx, cy) 00165 * \param[out] rgb_focal_length_x the RGB focal length (fx) 00166 * \param[out] rgb_focal_length_y the RGB focal length (fy) 00167 * \param[out] rgb_principal_point_x the RGB principal point (cx) 00168 * \param[out] rgb_principal_point_y the RGB principal point (cy) 00169 */ 00170 inline void 00171 getRGBCameraIntrinsics (double &rgb_focal_length_x, 00172 double &rgb_focal_length_y, 00173 double &rgb_principal_point_x, 00174 double &rgb_principal_point_y) const 00175 { 00176 rgb_focal_length_x = rgb_focal_length_x_; 00177 rgb_focal_length_y = rgb_focal_length_y_; 00178 rgb_principal_point_x = rgb_principal_point_x_; 00179 rgb_principal_point_y = rgb_principal_point_y_; 00180 } 00181 00182 00183 /** \brief Set the RGB image focal length (fx = fy). 00184 * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy) 00185 * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it 00186 * and the grabber will use the default values from the camera instead. 00187 * These parameters will be used for XYZRGBA clouds. 00188 */ 00189 inline void 00190 setRGBFocalLength (const double rgb_focal_length) 00191 { 00192 rgb_focal_length_x_ = rgb_focal_length_y_ = rgb_focal_length; 00193 } 00194 00195 /** \brief Set the RGB image focal length 00196 * \param[in] rgb_focal_length_x the RGB focal length (fx) 00197 * \param[in] rgb_focal_ulength_y the RGB focal length (fy) 00198 * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them 00199 * and the grabber will use the default values from the camera instead. 00200 * These parameters will be used for XYZRGBA clouds. 00201 */ 00202 inline void 00203 setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y) 00204 { 00205 rgb_focal_length_x_ = rgb_focal_length_x; 00206 rgb_focal_length_y_ = rgb_focal_length_y; 00207 } 00208 00209 /** \brief Return the RGB focal length parameters (fx, fy) 00210 * \param[out] rgb_focal_length_x the RGB focal length (fx) 00211 * \param[out] rgb_focal_length_y the RGB focal length (fy) 00212 */ 00213 inline void 00214 getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const 00215 { 00216 rgb_focal_length_x = rgb_focal_length_x_; 00217 rgb_focal_length_y = rgb_focal_length_y_; 00218 } 00219 00220 /** \brief Set the Depth camera parameters (fx, fy, cx, cy) 00221 * \param[in] depth_focal_length_x the Depth focal length (fx) 00222 * \param[in] depth_focal_length_y the Depth focal length (fy) 00223 * \param[in] depth_principal_point_x the Depth principal point (cx) 00224 * \param[in] depth_principal_point_y the Depth principal point (cy) 00225 * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them 00226 * and the grabber will use the default values from the camera instead. 00227 */ 00228 inline void 00229 setDepthCameraIntrinsics (const double depth_focal_length_x, 00230 const double depth_focal_length_y, 00231 const double depth_principal_point_x, 00232 const double depth_principal_point_y) 00233 { 00234 depth_focal_length_x_ = depth_focal_length_x; 00235 depth_focal_length_y_ = depth_focal_length_y; 00236 depth_principal_point_x_ = depth_principal_point_x; 00237 depth_principal_point_y_ = depth_principal_point_y; 00238 } 00239 00240 /** \brief Get the Depth camera parameters (fx, fy, cx, cy) 00241 * \param[out] depth_focal_length_x the Depth focal length (fx) 00242 * \param[out] depth_focal_length_y the Depth focal length (fy) 00243 * \param[out] depth_principal_point_x the Depth principal point (cx) 00244 * \param[out] depth_principal_point_y the Depth principal point (cy) 00245 */ 00246 inline void 00247 getDepthCameraIntrinsics (double &depth_focal_length_x, 00248 double &depth_focal_length_y, 00249 double &depth_principal_point_x, 00250 double &depth_principal_point_y) const 00251 { 00252 depth_focal_length_x = depth_focal_length_x_; 00253 depth_focal_length_y = depth_focal_length_y_; 00254 depth_principal_point_x = depth_principal_point_x_; 00255 depth_principal_point_y = depth_principal_point_y_; 00256 } 00257 00258 /** \brief Set the Depth image focal length (fx = fy). 00259 * \param[in] depth_focal_length the Depth focal length (assumes fx = fy) 00260 * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it 00261 * and the grabber will use the default values from the camera instead. 00262 */ 00263 inline void 00264 setDepthFocalLength (const double depth_focal_length) 00265 { 00266 depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length; 00267 } 00268 00269 00270 /** \brief Set the Depth image focal length 00271 * \param[in] depth_focal_length_x the Depth focal length (fx) 00272 * \param[in] depth_focal_length_y the Depth focal length (fy) 00273 * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them 00274 * and the grabber will use the default values from the camera instead. 00275 */ 00276 inline void 00277 setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y) 00278 { 00279 depth_focal_length_x_ = depth_focal_length_x; 00280 depth_focal_length_y_ = depth_focal_length_y; 00281 } 00282 00283 /** \brief Return the Depth focal length parameters (fx, fy) 00284 * \param[out] depth_focal_length_x the Depth focal length (fx) 00285 * \param[out] depth_focal_length_y the Depth focal length (fy) 00286 */ 00287 inline void 00288 getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const 00289 { 00290 depth_focal_length_x = depth_focal_length_x_; 00291 depth_focal_length_y = depth_focal_length_y_; 00292 } 00293 00294 /** \brief Convert vector of raw shift values to depth values 00295 * \param[in] shift_data_ptr input shift data 00296 * \param[out] depth_data_ptr generated depth data 00297 * \param[in] size of shift and depth buffer 00298 */ 00299 inline void 00300 convertShiftToDepth ( 00301 const uint16_t* shift_data_ptr, 00302 uint16_t* depth_data_ptr, 00303 std::size_t size) const 00304 { 00305 // get openni device instance 00306 boost::shared_ptr<openni_wrapper::OpenNIDevice> openni_device = 00307 this->getDevice (); 00308 00309 const uint16_t* shift_data_it = shift_data_ptr; 00310 uint16_t* depth_data_it = depth_data_ptr; 00311 00312 // shift-to-depth lookup 00313 for (std::size_t i=0; i<size; ++i) 00314 { 00315 *depth_data_it = openni_device->shiftToDepth(*shift_data_it); 00316 00317 shift_data_it++; 00318 depth_data_it++; 00319 } 00320 00321 } 00322 00323 00324 protected: 00325 /** \brief On initialization processing. */ 00326 void 00327 onInit (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); 00328 00329 /** \brief Sets up an OpenNI device. */ 00330 void 00331 setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); 00332 00333 /** \brief Update mode maps. */ 00334 void 00335 updateModeMaps (); 00336 00337 /** \brief Start synchronization. */ 00338 void 00339 startSynchronization (); 00340 00341 /** \brief Stop synchronization. */ 00342 void 00343 stopSynchronization (); 00344 00345 /** \brief Map config modes. */ 00346 bool 00347 mapConfigMode2XnMode (int mode, XnMapOutputMode &xnmode) const; 00348 00349 // callback methods 00350 /** \brief RGB image callback. */ 00351 virtual void 00352 imageCallback (boost::shared_ptr<openni_wrapper::Image> image, void* cookie); 00353 00354 /** \brief Depth image callback. */ 00355 virtual void 00356 depthCallback (boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie); 00357 00358 /** \brief IR image callback. */ 00359 virtual void 00360 irCallback (boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie); 00361 00362 /** \brief RGB + Depth image callback. */ 00363 virtual void 00364 imageDepthImageCallback (const boost::shared_ptr<openni_wrapper::Image> &image, 00365 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image); 00366 00367 /** \brief IR + Depth image callback. */ 00368 virtual void 00369 irDepthImageCallback (const boost::shared_ptr<openni_wrapper::IRImage> &image, 00370 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image); 00371 00372 /** \brief Process changed signals. */ 00373 virtual void 00374 signalsChanged (); 00375 00376 // helper methods 00377 00378 /** \brief Check if the RGB and Depth images are required to be synchronized or not. */ 00379 virtual void 00380 checkImageAndDepthSynchronizationRequired (); 00381 00382 /** \brief Check if the RGB image stream is required or not. */ 00383 virtual void 00384 checkImageStreamRequired (); 00385 00386 /** \brief Check if the depth stream is required or not. */ 00387 virtual void 00388 checkDepthStreamRequired (); 00389 00390 /** \brief Check if the IR image stream is required or not. */ 00391 virtual void 00392 checkIRStreamRequired (); 00393 00394 00395 /** \brief Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ> 00396 * \param[in] depth the depth image to convert 00397 */ 00398 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > 00399 convertToXYZPointCloud (const boost::shared_ptr<openni_wrapper::DepthImage> &depth) const; 00400 00401 /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud<PointT> 00402 * \param[in] image the RGB image to convert 00403 * \param[in] depth_image the depth image to convert 00404 */ 00405 template <typename PointT> typename pcl::PointCloud<PointT>::Ptr 00406 convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image, 00407 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00408 00409 /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud<pcl::PointXYZI> 00410 * \param[in] image the IR image to convert 00411 * \param[in] depth_image the depth image to convert 00412 */ 00413 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > 00414 convertToXYZIPointCloud (const boost::shared_ptr<openni_wrapper::IRImage> &image, 00415 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00416 00417 00418 Synchronizer<boost::shared_ptr<openni_wrapper::Image>, boost::shared_ptr<openni_wrapper::DepthImage> > rgb_sync_; 00419 Synchronizer<boost::shared_ptr<openni_wrapper::IRImage>, boost::shared_ptr<openni_wrapper::DepthImage> > ir_sync_; 00420 00421 /** \brief The actual openni device. */ 00422 boost::shared_ptr<openni_wrapper::OpenNIDevice> device_; 00423 00424 std::string rgb_frame_id_; 00425 std::string depth_frame_id_; 00426 unsigned image_width_; 00427 unsigned image_height_; 00428 unsigned depth_width_; 00429 unsigned depth_height_; 00430 00431 bool image_required_; 00432 bool depth_required_; 00433 bool ir_required_; 00434 bool sync_required_; 00435 00436 boost::signals2::signal<sig_cb_openni_image>* image_signal_; 00437 boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_; 00438 boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_; 00439 boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_; 00440 boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_; 00441 boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_; 00442 boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_; 00443 boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_; 00444 boost::signals2::signal<sig_cb_openni_point_cloud_rgba>* point_cloud_rgba_signal_; 00445 00446 struct modeComp 00447 { 00448 00449 bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode & mode2) const 00450 { 00451 if (mode1.nXRes < mode2.nXRes) 00452 return true; 00453 else if (mode1.nXRes > mode2.nXRes) 00454 return false; 00455 else if (mode1.nYRes < mode2.nYRes) 00456 return true; 00457 else if (mode1.nYRes > mode2.nYRes) 00458 return false; 00459 else if (mode1.nFPS < mode2.nFPS) 00460 return true; 00461 else 00462 return false; 00463 } 00464 } ; 00465 std::map<int, XnMapOutputMode> config2xn_map_; 00466 00467 openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle; 00468 openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle; 00469 openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle; 00470 bool running_; 00471 00472 /** \brief The RGB image focal length (fx). */ 00473 double rgb_focal_length_x_; 00474 /** \brief The RGB image focal length (fy). */ 00475 double rgb_focal_length_y_; 00476 /** \brief The RGB image principal point (cx). */ 00477 double rgb_principal_point_x_; 00478 /** \brief The RGB image principal point (cy). */ 00479 double rgb_principal_point_y_; 00480 /** \brief The depth image focal length (fx). */ 00481 double depth_focal_length_x_; 00482 /** \brief The depth image focal length (fy). */ 00483 double depth_focal_length_y_; 00484 /** \brief The depth image principal point (cx). */ 00485 double depth_principal_point_x_; 00486 /** \brief The depth image principal point (cy). */ 00487 double depth_principal_point_y_; 00488 00489 public: 00490 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00491 }; 00492 00493 boost::shared_ptr<openni_wrapper::OpenNIDevice> 00494 OpenNIGrabber::getDevice () const 00495 { 00496 return device_; 00497 } 00498 00499 } // namespace pcl 00500 #endif // __PCL_IO_OPENNI_GRABBER__ 00501 #endif // HAVE_OPENNI