|
Point Cloud Library (PCL)
1.5.1
|
|
Point Cloud Library (PCL)
1.5.1
|
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 PointXYZRGBA; 00061 struct PointXYZI; 00062 template <typename T> class PointCloud; 00063 00068 class PCL_EXPORTS OpenNIGrabber : public Grabber 00069 { 00070 public: 00071 00072 typedef enum 00073 { 00074 OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz 00075 OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect 00076 OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect 00077 OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion 00078 OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion 00079 OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect 00080 OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion 00081 OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN) 00082 OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN) 00083 OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN) 00084 } Mode; 00085 00086 //define callback signature typedefs 00087 typedef void (sig_cb_openni_image) (const boost::shared_ptr<openni_wrapper::Image>&); 00088 typedef void (sig_cb_openni_depth_image) (const boost::shared_ptr<openni_wrapper::DepthImage>&); 00089 typedef void (sig_cb_openni_ir_image) (const boost::shared_ptr<openni_wrapper::IRImage>&); 00090 typedef void (sig_cb_openni_image_depth_image) (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ; 00091 typedef void (sig_cb_openni_ir_depth_image) (const boost::shared_ptr<openni_wrapper::IRImage>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ; 00092 typedef void (sig_cb_openni_point_cloud) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&); 00093 typedef void (sig_cb_openni_point_cloud_rgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB> >&); 00094 typedef void (sig_cb_openni_point_cloud_rgba) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&); 00095 typedef void (sig_cb_openni_point_cloud_i) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&); 00096 typedef void (sig_cb_openni_point_cloud_eigen) (const boost::shared_ptr<const pcl::PointCloud<Eigen::MatrixXf> >&); 00097 00098 public: 00104 OpenNIGrabber (const std::string& device_id = "", 00105 const Mode& depth_mode = OpenNI_Default_Mode, 00106 const Mode& image_mode = OpenNI_Default_Mode); 00107 00109 virtual ~OpenNIGrabber () throw (); 00110 00112 virtual void 00113 start (); 00114 00116 virtual void 00117 stop (); 00118 00120 virtual bool 00121 isRunning () const; 00122 00123 virtual std::string 00124 getName () const; 00125 00127 virtual float 00128 getFramesPerSecond () const; 00129 00131 inline boost::shared_ptr<openni_wrapper::OpenNIDevice> 00132 getDevice () const; 00133 00135 std::vector<std::pair<int, XnMapOutputMode> > 00136 getAvailableDepthModes () const; 00137 00139 std::vector<std::pair<int, XnMapOutputMode> > 00140 getAvailableImageModes () const; 00141 00142 private: 00144 void 00145 onInit (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); 00146 00148 void 00149 setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); 00150 00152 void 00153 updateModeMaps (); 00154 00156 void 00157 startSynchronization (); 00158 00160 void 00161 stopSynchronization (); 00162 00163 00165 bool 00166 mapConfigMode2XnMode (int mode, XnMapOutputMode &xnmode) const; 00167 00168 // callback methods 00170 void 00171 imageCallback (boost::shared_ptr<openni_wrapper::Image> image, void* cookie); 00172 00174 void 00175 depthCallback (boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie); 00176 00178 void 00179 irCallback (boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie); 00180 00182 void 00183 imageDepthImageCallback (const boost::shared_ptr<openni_wrapper::Image> &image, 00184 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image); 00185 00187 void 00188 irDepthImageCallback (const boost::shared_ptr<openni_wrapper::IRImage> &image, 00189 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image); 00190 00192 virtual void 00193 signalsChanged (); 00194 00195 // helper methods 00196 00198 virtual inline void 00199 checkImageAndDepthSynchronizationRequired (); 00200 00202 virtual inline void 00203 checkImageStreamRequired (); 00204 00206 virtual inline void 00207 checkDepthStreamRequired (); 00208 00210 virtual inline void 00211 checkIRStreamRequired (); 00212 00214 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > 00215 convertToXYZPointCloud (const boost::shared_ptr<openni_wrapper::DepthImage> &depth) const; 00216 00218 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > 00219 convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image, 00220 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00221 00223 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > 00224 convertToXYZRGBAPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image, 00225 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00227 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > 00228 convertToXYZIPointCloud (const boost::shared_ptr<openni_wrapper::IRImage> &image, 00229 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00230 00231 Synchronizer<boost::shared_ptr<openni_wrapper::Image>, boost::shared_ptr<openni_wrapper::DepthImage> > rgb_sync_; 00232 Synchronizer<boost::shared_ptr<openni_wrapper::IRImage>, boost::shared_ptr<openni_wrapper::DepthImage> > ir_sync_; 00233 00239 boost::shared_ptr<pcl::PointCloud<Eigen::MatrixXf> > 00240 convertToEigenPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image, 00241 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const; 00242 00244 boost::shared_ptr<openni_wrapper::OpenNIDevice> device_; 00245 00246 std::string rgb_frame_id_; 00247 std::string depth_frame_id_; 00248 unsigned image_width_; 00249 unsigned image_height_; 00250 unsigned depth_width_; 00251 unsigned depth_height_; 00252 00253 bool image_required_; 00254 bool depth_required_; 00255 bool ir_required_; 00256 bool sync_required_; 00257 00258 boost::signals2::signal<sig_cb_openni_image>* image_signal_; 00259 boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_; 00260 boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_; 00261 boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_; 00262 boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_; 00263 boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_; 00264 boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_; 00265 boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_; 00266 boost::signals2::signal<sig_cb_openni_point_cloud_rgba>* point_cloud_rgba_signal_; 00267 boost::signals2::signal<sig_cb_openni_point_cloud_eigen>* point_cloud_eigen_signal_; 00268 00269 struct modeComp 00270 { 00271 00272 bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode & mode2) const 00273 { 00274 if (mode1.nXRes < mode2.nXRes) 00275 return true; 00276 else if (mode1.nXRes > mode2.nXRes) 00277 return false; 00278 else if (mode1.nYRes < mode2.nYRes) 00279 return true; 00280 else if (mode1.nYRes > mode2.nYRes) 00281 return false; 00282 else if (mode1.nFPS < mode2.nFPS) 00283 return true; 00284 else 00285 return false; 00286 } 00287 } ; 00288 std::map<int, XnMapOutputMode> config2xn_map_; 00289 00290 openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle; 00291 openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle; 00292 openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle; 00293 bool running_; 00294 00295 public: 00296 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00297 }; 00298 00299 boost::shared_ptr<openni_wrapper::OpenNIDevice> 00300 OpenNIGrabber::getDevice () const 00301 { 00302 return device_; 00303 } 00304 00305 } // namespace pcl 00306 #endif // __PCL_IO_OPENNI_GRABBER__ 00307 #endif // HAVE_OPENNI
1.8.0
