|
Point Cloud Library (PCL)
1.4.0
|
|
Point Cloud Library (PCL)
1.4.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 * 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 00038 #include "pcl/pcl_config.h" 00039 #ifdef HAVE_OPENNI 00040 00041 #ifndef __PCL_IO_OPENNI_GRABBER__ 00042 #define __PCL_IO_OPENNI_GRABBER__ 00043 00044 #include <Eigen/Core> 00045 #include <pcl/io/grabber.h> 00046 #include <pcl/io/openni_camera/openni_driver.h> 00047 #include <pcl/io/openni_camera/openni_device_kinect.h> 00048 #include <pcl/io/openni_camera/openni_image.h> 00049 #include <pcl/io/openni_camera/openni_depth_image.h> 00050 #include <pcl/io/openni_camera/openni_ir_image.h> 00051 #include <string> 00052 #include <deque> 00053 #include <boost/thread/mutex.hpp> 00054 #include <pcl/common/synchronizer.h> 00055 00056 namespace pcl 00057 { 00058 struct PointXYZ; 00059 struct PointXYZRGB; 00060 struct PointXYZI; 00061 template <typename T> class PointCloud; 00062 00067 class PCL_EXPORTS OpenNIGrabber : public Grabber 00068 { 00069 public: 00070 00071 typedef enum 00072 { 00073 OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz 00074 OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect 00075 OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect 00076 OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion 00077 OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion 00078 OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect 00079 OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion 00080 OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN) 00081 OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN) 00082 OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN) 00083 } Mode; 00084 00085 //define callback signature typedefs 00086 typedef void (sig_cb_openni_image) (const boost::shared_ptr<openni_wrapper::Image>&); 00087 typedef void (sig_cb_openni_depth_image) (const boost::shared_ptr<openni_wrapper::DepthImage>&); 00088 typedef void (sig_cb_openni_ir_image) (const boost::shared_ptr<openni_wrapper::IRImage>&); 00089 typedef void (sig_cb_openni_image_depth_image) (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ; 00090 typedef void (sig_cb_openni_ir_depth_image) (const boost::shared_ptr<openni_wrapper::IRImage>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ; 00091 typedef void (sig_cb_openni_point_cloud) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&); 00092 typedef void (sig_cb_openni_point_cloud_rgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB> >&); 00093 typedef void (sig_cb_openni_point_cloud_i) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&); 00094 typedef void (sig_cb_openni_point_cloud_eigen) (const boost::shared_ptr<const pcl::PointCloud<Eigen::MatrixXf> >&); 00095 00096 public: 00102 OpenNIGrabber (const std::string& device_id = "", 00103 const Mode& depth_mode = OpenNI_Default_Mode, 00104 const Mode& image_mode = OpenNI_Default_Mode); 00105 00107 virtual ~OpenNIGrabber () throw (); 00108 00110 virtual void 00111 start (); 00112 00114 virtual void 00115 stop (); 00116 00118 virtual bool 00119 isRunning () const; 00120 00121 virtual std::string 00122 getName () const; 00123 00125 virtual float 00126 getFramesPerSecond () const; 00127 00129 inline boost::shared_ptr<openni_wrapper::OpenNIDevice> 00130 getDevice () const; 00131 00133 std::vector<std::pair<int, XnMapOutputMode> > 00134 getAvailableDepthModes () const; 00135 00137 std::vector<std::pair<int, XnMapOutputMode> > 00138 getAvailableImageModes () const; 00139 00144 void 00145 setPrincipalPoint (float cx, float cy); 00146 00150 void 00151 setAspectRatio (float aspect_ratio); 00152 00156 void 00157 setFocalLength (float focal_length); 00158 00165 void 00166 setLensDistortion (float k1, float k2, float t1, float t2); 00167 00171 float 00172 getFocalLength (unsigned image_width) const; 00173 00174 private: 00176 void 00177 onInit (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); 00178 00180 void 00181 setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); 00182 00184 void 00185 updateModeMaps (); 00186 00188 void 00189 startSynchronization (); 00190 00192 void 00193 stopSynchronization (); 00194 00195 00197 bool 00198 mapConfigMode2XnMode (int mode, XnMapOutputMode &xnmode) const; 00199 00200 // callback methods 00202 void 00203 imageCallback (boost::shared_ptr<openni_wrapper::Image> image, void* cookie); 00204 00206 void 00207 depthCallback (boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie); 00208 00210 void 00211 irCallback (boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie); 00212 00214 void 00215 imageDepthImageCallback (const boost::shared_ptr<openni_wrapper::Image> &image, 00216 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image); 00217 00219 void 00220 irDepthImageCallback (const boost::shared_ptr<openni_wrapper::IRImage> &image, 00221 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image); 00222 00224 virtual void 00225 signalsChanged (); 00226 00227 // helper methods 00228 00230 virtual inline void 00231 checkImageAndDepthSynchronizationRequired (); 00232 00234 virtual inline void 00235 checkImageStreamRequired (); 00236 00238 virtual inline void 00239 checkDepthStreamRequired (); 00240 00242 virtual inline void 00243 checkIRStreamRequired (); 00244 00246 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > 00247 convertToXYZPointCloud (const boost::shared_ptr<openni_wrapper::DepthImage> &depth) const; 00248 00250 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > 00251 convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image, 00252 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00254 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > 00255 convertToXYZIPointCloud (const boost::shared_ptr<openni_wrapper::IRImage> &image, 00256 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00257 00258 Synchronizer<boost::shared_ptr<openni_wrapper::Image>, boost::shared_ptr<openni_wrapper::DepthImage> > rgb_sync_; 00259 Synchronizer<boost::shared_ptr<openni_wrapper::IRImage>, boost::shared_ptr<openni_wrapper::DepthImage> > ir_sync_; 00260 00266 boost::shared_ptr<pcl::PointCloud<Eigen::MatrixXf> > 00267 convertToEigenPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image, 00268 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00269 00271 boost::shared_ptr<openni_wrapper::OpenNIDevice> device_; 00272 00273 std::string rgb_frame_id_; 00274 std::string depth_frame_id_; 00275 unsigned image_width_; 00276 unsigned image_height_; 00277 unsigned depth_width_; 00278 unsigned depth_height_; 00279 00280 bool image_required_; 00281 bool depth_required_; 00282 bool ir_required_; 00283 bool sync_required_; 00284 00285 boost::signals2::signal<sig_cb_openni_image>* image_signal_; 00286 boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_; 00287 boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_; 00288 boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_; 00289 boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_; 00290 boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_; 00291 boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_; 00292 boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_; 00293 boost::signals2::signal<sig_cb_openni_point_cloud_eigen>* point_cloud_eigen_signal_; 00294 00295 struct modeComp 00296 { 00297 00298 bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode & mode2) const 00299 { 00300 if (mode1.nXRes < mode2.nXRes) 00301 return true; 00302 else if (mode1.nXRes > mode2.nXRes) 00303 return false; 00304 else if (mode1.nYRes < mode2.nYRes) 00305 return true; 00306 else if (mode1.nYRes > mode2.nYRes) 00307 return false; 00308 else if (mode1.nFPS < mode2.nFPS) 00309 return true; 00310 else 00311 return false; 00312 } 00313 } ; 00314 std::map<int, XnMapOutputMode> config2xn_map_; 00315 00316 openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle; 00317 openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle; 00318 openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle; 00319 bool running_; 00320 00321 public: 00322 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 00323 } ; 00324 00325 boost::shared_ptr<openni_wrapper::OpenNIDevice> 00326 OpenNIGrabber::getDevice () const 00327 { 00328 return device_; 00329 } 00330 00331 } // namespace pcl 00332 #endif // __PCL_IO_OPENNI_GRABBER__ 00333 #endif // HAVE_OPENNI
1.7.6.1
