Point Cloud Library (PCL)  1.3.1
openni_grabber.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * Author: Nico Blodow (blodow@cs.tum.edu), Suat Gedikli (gedikli@willowgarage.com)
00035  */
00036 
00037 #include "pcl/pcl_config.h"
00038 #ifdef HAVE_OPENNI
00039 
00040 #ifndef __PCL_IO_OPENNI_GRABBER__
00041 #define __PCL_IO_OPENNI_GRABBER__
00042 
00043 #include <Eigen/Core>
00044 #include <pcl/io/grabber.h>
00045 #include <pcl/io/openni_camera/openni_driver.h>
00046 #include <pcl/io/openni_camera/openni_device_kinect.h>
00047 #include <pcl/io/openni_camera/openni_image.h>
00048 #include <pcl/io/openni_camera/openni_depth_image.h>
00049 #include <pcl/io/openni_camera/openni_ir_image.h>
00050 #include <string>
00051 #include <deque>
00052 #include <boost/thread/mutex.hpp>
00053 #include <pcl/common/synchronizer.h>
00054 
00055 namespace pcl
00056 {
00057   struct PointXYZ;
00058   struct PointXYZRGB;
00059   struct PointXYZI;
00060   template <typename T> class PointCloud;
00061 
00065   class PCL_EXPORTS OpenNIGrabber : public Grabber
00066   {
00067     public:
00068 
00069       typedef enum
00070       {
00071         OpenNI_Default_Mode = 0, /*This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz*/
00072         OpenNI_SXGA_15Hz = 1,  /*Only supported by the Kinect*/
00073         OpenNI_VGA_30Hz = 2,   /*Supported by PSDK, Xtion and Kinect*/
00074         OpenNI_VGA_25Hz = 3,   /*Supportged by PSDK and Xtion*/
00075         OpenNI_QVGA_25Hz = 4,  /*Supported by PSDK and Xtion*/
00076         OpenNI_QVGA_30Hz = 5,  /*Supported by PSDK, Xtion and Kinect*/
00077         OpenNI_QVGA_60Hz = 6,  /*Supported by PSDK and Xtion*/
00078         OpenNI_QQVGA_25Hz = 7, /*Not supported -> using software downsampling (only for integer scale factor and only NN)*/
00079         OpenNI_QQVGA_30Hz = 8, /*Not supported -> using software downsampling (only for integer scale factor and only NN)*/
00080         OpenNI_QQVGA_60Hz = 9  /*Not supported -> using software downsampling (only for integer scale factor and only NN)*/
00081       } Mode;
00082 
00083       //define callback signature typedefs
00084       typedef void (sig_cb_openni_image) (const boost::shared_ptr<openni_wrapper::Image>&);
00085       typedef void (sig_cb_openni_depth_image) (const boost::shared_ptr<openni_wrapper::DepthImage>&);
00086       typedef void (sig_cb_openni_ir_image) (const boost::shared_ptr<openni_wrapper::IRImage>&);
00087       typedef void (sig_cb_openni_image_depth_image) (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ;
00088       typedef void (sig_cb_openni_ir_depth_image) (const boost::shared_ptr<openni_wrapper::IRImage>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ;
00089       typedef void (sig_cb_openni_point_cloud) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
00090       typedef void (sig_cb_openni_point_cloud_rgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB> >&);
00091       typedef void (sig_cb_openni_point_cloud_i) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
00092 
00093     public:
00094       //enable using some openni parameters in the constructor.
00100       OpenNIGrabber (const std::string& device_id = "",
00101                      const Mode& depth_mode = OpenNI_Default_Mode,
00102                      const Mode& image_mode = OpenNI_Default_Mode);
00103 
00105       virtual ~OpenNIGrabber () throw ();
00106 
00108       virtual void
00109       start ();
00110 
00112       virtual void
00113       stop ();
00114 
00116       virtual bool
00117       isRunning () const;
00118 
00120       virtual std::string
00121       getName () const;
00122 
00124       inline boost::shared_ptr<openni_wrapper::OpenNIDevice>
00125       getDevice () const;
00126 
00128       std::vector<std::pair<int, XnMapOutputMode> >
00129       getAvailableDepthModes () const;
00130 
00132       std::vector<std::pair<int, XnMapOutputMode> >
00133       getAvailableImageModes () const;
00134 
00135       void setPrincipalPoint (float cx, float cy);
00136 
00137       void setAspectRatio (float aspect_ratio);
00138 
00139       void setFocalLength (float focal_length);
00140 
00141       void setLensDistortion (float k1, float k2, float t1, float t2);
00142 
00143       float getFocalLength (unsigned image_width) const;
00144 
00145     private:
00147       void
00148       onInit (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode);
00149 
00151       void
00152       setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode);
00153 
00155       void
00156       updateModeMaps ();
00157 
00159       void
00160       startSynchronization ();
00161 
00163       void
00164       stopSynchronization ();
00165 
00166 
00168       bool
00169       mapConfigMode2XnMode (int mode, XnMapOutputMode &xnmode) const;
00170 
00171       // callback methods
00173       void
00174       imageCallback (boost::shared_ptr<openni_wrapper::Image> image, void* cookie);
00175 
00177       void
00178       depthCallback (boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie);
00179 
00181       void
00182       irCallback (boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie);
00183 
00185       void
00186       imageDepthImageCallback (const boost::shared_ptr<openni_wrapper::Image> &image,
00187                                const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image);
00188 
00190       void
00191       irDepthImageCallback (const boost::shared_ptr<openni_wrapper::IRImage> &image,
00192                             const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image);
00193 
00195       virtual void
00196       signalsChanged ();
00197 
00198       // helper methods
00199 
00201       virtual inline void
00202       checkImageAndDepthSynchronizationRequired ();
00203 
00205       virtual inline void
00206       checkImageStreamRequired ();
00207 
00209       virtual inline void
00210       checkDepthStreamRequired ();
00211 
00213       virtual inline void
00214       checkIRStreamRequired();
00215 
00217       boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> >
00218       convertToXYZPointCloud (const boost::shared_ptr<openni_wrapper::DepthImage> &depth) const;
00219 
00221       boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >
00222       convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image,
00223                                  const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const;
00225       boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> >
00226       convertToXYZIPointCloud (const boost::shared_ptr<openni_wrapper::IRImage> &image,
00227                                const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const;
00228 
00229       Synchronizer<boost::shared_ptr<openni_wrapper::Image>, boost::shared_ptr<openni_wrapper::DepthImage> > rgb_sync_;
00230       Synchronizer<boost::shared_ptr<openni_wrapper::IRImage>, boost::shared_ptr<openni_wrapper::DepthImage> > ir_sync_;
00231 
00233       boost::shared_ptr<openni_wrapper::OpenNIDevice> device_;
00234 
00235       std::string rgb_frame_id_;
00236       std::string depth_frame_id_;
00237       unsigned image_width_;
00238       unsigned image_height_;
00239       unsigned depth_width_;
00240       unsigned depth_height_;
00241 
00242       bool image_required_;
00243       bool depth_required_;
00244       bool ir_required_;
00245       bool sync_required_;
00246 
00247       boost::signals2::signal<sig_cb_openni_image >* image_signal_;
00248       boost::signals2::signal<sig_cb_openni_depth_image >* depth_image_signal_;
00249       boost::signals2::signal<sig_cb_openni_ir_image >* ir_image_signal_;
00250       boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_;
00251       boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_;
00252       boost::signals2::signal<sig_cb_openni_point_cloud >* point_cloud_signal_;
00253       boost::signals2::signal<sig_cb_openni_point_cloud_i >* point_cloud_i_signal_;
00254       boost::signals2::signal<sig_cb_openni_point_cloud_rgb >* point_cloud_rgb_signal_;
00255 
00256       struct modeComp
00257       {
00258 
00259         bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode & mode2) const
00260         {
00261           if (mode1.nXRes < mode2.nXRes)
00262             return true;
00263           else if (mode1.nXRes > mode2.nXRes)
00264             return false;
00265           else if (mode1.nYRes < mode2.nYRes)
00266             return true;
00267           else if (mode1.nYRes > mode2.nYRes)
00268             return false;
00269           else if (mode1.nFPS < mode2.nFPS)
00270             return true;
00271           else
00272             return false;
00273         }
00274       } ;
00275       std::map<int, XnMapOutputMode> config2xn_map_;
00276 
00277       openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle;
00278       openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle;
00279       openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle;
00280       bool running_;
00281 
00282     public:
00283       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00284   } ;
00285 
00286   boost::shared_ptr<openni_wrapper::OpenNIDevice>
00287   OpenNIGrabber::getDevice () const
00288   {
00289     return device_;
00290   }
00291 
00292 } // namespace pcl
00293 #endif // __PCL_IO_OPENNI_GRABBER__
00294 #endif // HAVE_OPENNI
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines