Point Cloud Library (PCL)
1.3.1
|
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