Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/io/include/pcl/io/openni_grabber.h
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