Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011 Willow Garage, Inc. 00005 * Suat Gedikli <gedikli@willowgarage.com> 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 __OPENNI_IDEVICE_H__ 00042 #define __OPENNI_IDEVICE_H__ 00043 #include <map> 00044 #include <vector> 00045 #include <utility> 00046 #include "openni_exception.h" 00047 #include <XnCppWrapper.h> 00048 #include <boost/noncopyable.hpp> 00049 #include <boost/function.hpp> 00050 #include <boost/thread.hpp> 00051 #include <boost/thread/condition.hpp> 00052 00053 #include <pcl/pcl_macros.h> 00054 00056 00057 #ifndef _WIN32 00058 #define __stdcall 00059 #endif 00060 00061 namespace openni_wrapper 00062 { 00063 class Image; 00064 class DepthImage; 00065 class IRImage; 00072 class PCL_EXPORTS OpenNIDevice : public boost::noncopyable 00073 { 00074 public: 00075 typedef boost::function<void(boost::shared_ptr<Image>, void* cookie) > ImageCallbackFunction; 00076 typedef boost::function<void(boost::shared_ptr<DepthImage>, void* cookie) > DepthImageCallbackFunction; 00077 typedef boost::function<void(boost::shared_ptr<IRImage>, void* cookie) > IRImageCallbackFunction; 00078 typedef unsigned CallbackHandle; 00079 00080 public: 00081 virtual ~OpenNIDevice () throw (); 00082 00083 bool findCompatibleImageMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw (); 00084 bool findCompatibleDepthMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw (); 00085 00086 bool isImageModeSupported (const XnMapOutputMode& output_mode) const throw (); 00087 bool isDepthModeSupported (const XnMapOutputMode& output_mode) const throw (); 00088 00089 const XnMapOutputMode& getDefaultImageMode () const throw (); 00090 const XnMapOutputMode& getDefaultDepthMode () const throw (); 00091 const XnMapOutputMode& getDefaultIRMode () const throw (); 00092 00093 void setImageOutputMode (const XnMapOutputMode& output_mode); 00094 void setDepthOutputMode (const XnMapOutputMode& output_mode); 00095 void setIROutputMode (const XnMapOutputMode& output_mode); 00096 00097 XnMapOutputMode getImageOutputMode () const; 00098 XnMapOutputMode getDepthOutputMode () const; 00099 XnMapOutputMode getIROutputMode () const; 00100 00101 void setDepthRegistration (bool on_off); 00102 bool isDepthRegistered () const throw (); 00103 bool isDepthRegistrationSupported () const throw (); 00104 00105 void setSynchronization (bool on_off); 00106 bool isSynchronized () const throw (); 00107 bool isSynchronizationSupported () const throw (); 00108 00109 // just supported by primesense -> virtual 00110 bool isDepthCropped () const; 00111 void setDepthCropping (unsigned x, unsigned y, unsigned width, unsigned height); 00112 bool isDepthCroppingSupported () const throw (); 00113 00117 inline float getImageFocalLength (int output_x_resolution = 0) const throw (); 00118 00122 inline float getDepthFocalLength (int output_x_resolution = 0) const throw (); 00123 inline float getBaseline () const throw (); 00124 00125 virtual void startImageStream (); 00126 virtual void stopImageStream (); 00127 00128 virtual void startDepthStream (); 00129 virtual void stopDepthStream (); 00130 00131 virtual void startIRStream (); 00132 virtual void stopIRStream (); 00133 00134 bool hasImageStream () const throw (); 00135 bool hasDepthStream () const throw (); 00136 bool hasIRStream () const throw (); 00137 00138 virtual bool isImageStreamRunning () const throw (); 00139 virtual bool isDepthStreamRunning () const throw (); 00140 virtual bool isIRStreamRunning () const throw (); 00141 00142 CallbackHandle registerImageCallback (const ImageCallbackFunction& callback, void* cookie = NULL) throw (); 00143 template<typename T> CallbackHandle registerImageCallback (void (T::*callback)(boost::shared_ptr<Image>, void* cookie), T& instance, void* cookie = NULL) throw (); 00144 bool unregisterImageCallback (const CallbackHandle& callbackHandle) throw (); 00145 00146 CallbackHandle registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = NULL) throw (); 00147 template<typename T> CallbackHandle registerDepthCallback (void (T::*callback)(boost::shared_ptr<DepthImage>, void* cookie), T& instance, void* cookie = NULL) throw (); 00148 bool unregisterDepthCallback (const CallbackHandle& callbackHandle) throw (); 00149 00150 CallbackHandle registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = NULL) throw (); 00151 template<typename T> CallbackHandle registerIRCallback (void (T::*callback)(boost::shared_ptr<IRImage>, void* cookie), T& instance, void* cookie = NULL) throw (); 00152 bool unregisterIRCallback (const CallbackHandle& callbackHandle) throw (); 00153 00157 const char* getSerialNumber () const throw (); 00159 const char* getConnectionString () const throw (); 00160 00161 const char* getVendorName () const throw (); 00162 const char* getProductName () const throw (); 00163 unsigned short getVendorID () const throw (); 00164 unsigned short getProductID () const throw (); 00165 unsigned char getBus () const throw (); 00166 unsigned char getAddress () const throw (); 00167 protected: 00168 typedef boost::function<void(boost::shared_ptr<Image>) > ActualImageCallbackFunction; 00169 typedef boost::function<void(boost::shared_ptr<DepthImage>) > ActualDepthImageCallbackFunction; 00170 typedef boost::function<void(boost::shared_ptr<IRImage>) > ActualIRImageCallbackFunction; 00171 00172 OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); 00173 OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); 00174 OpenNIDevice (xn::Context& context); 00175 static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw (); 00176 static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) throw (); 00177 static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) throw (); 00178 00179 // This is a workaround, since in the NewDepthDataAvailable function WaitAndUpdateData leads to a dead-lock behaviour 00180 // and retrieving image data without WaitAndUpdateData leads to incomplete images!!! 00181 void ImageDataThreadFunction (); 00182 void DepthDataThreadFunction (); 00183 void IRDataThreadFunction (); 00184 00185 virtual bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () = 0; 00186 00187 void setRegistration (bool on_off); 00188 virtual boost::shared_ptr<Image> getCurrentImage (boost::shared_ptr<xn::ImageMetaData> image_data) const throw () = 0; 00189 00190 void Init (); 00191 // holds the callback functions together with custom data 00192 // since same callback function can be registered multiple times with e.g. different custom data 00193 // we use a map structure with a handle as the key 00194 std::map< CallbackHandle, ActualImageCallbackFunction > image_callback_; 00195 std::map< CallbackHandle, ActualDepthImageCallbackFunction > depth_callback_; 00196 std::map< CallbackHandle, ActualIRImageCallbackFunction > ir_callback_; 00197 00198 std::vector<XnMapOutputMode> available_image_modes_; 00199 std::vector<XnMapOutputMode> available_depth_modes_; 00200 00202 xn::Context& context_; 00204 xn::NodeInfo device_node_info_; 00205 00207 xn::DepthGenerator depth_generator_; 00209 xn::ImageGenerator image_generator_; 00211 xn::IRGenerator ir_generator_; 00212 00213 XnCallbackHandle depth_callback_handle_; 00214 XnCallbackHandle image_callback_handle_; 00215 XnCallbackHandle ir_callback_handle_; 00216 00218 float depth_focal_length_SXGA_; 00220 float baseline_; 00222 static const float rgb_focal_length_SXGA_; 00223 00225 XnUInt64 shadow_value_; 00227 XnUInt64 no_sample_value_; 00228 00229 OpenNIDevice::CallbackHandle image_callback_handle_counter_; 00230 OpenNIDevice::CallbackHandle depth_callback_handle_counter_; 00231 OpenNIDevice::CallbackHandle ir_callback_handle_counter_; 00232 00233 bool quit_; 00234 mutable boost::mutex image_mutex_; 00235 mutable boost::mutex depth_mutex_; 00236 mutable boost::mutex ir_mutex_; 00237 boost::condition_variable image_condition_; 00238 boost::condition_variable depth_condition_; 00239 boost::condition_variable ir_condition_; 00240 boost::thread image_thread_; 00241 boost::thread depth_thread_; 00242 boost::thread ir_thread_; 00243 }; 00244 00245 float OpenNIDevice::getImageFocalLength (int output_x_resolution) const throw () 00246 { 00247 if (output_x_resolution == 0) 00248 output_x_resolution = getImageOutputMode ().nXRes; 00249 00250 float scale = output_x_resolution / (float)XN_SXGA_X_RES; 00251 return rgb_focal_length_SXGA_ * scale; 00252 } 00253 00254 float OpenNIDevice::getDepthFocalLength (int output_x_resolution) const throw () 00255 { 00256 if (output_x_resolution == 0) 00257 output_x_resolution = getDepthOutputMode ().nXRes; 00258 00259 float scale = output_x_resolution / (float)XN_SXGA_X_RES; 00260 if (isDepthRegistered ()) 00261 return rgb_focal_length_SXGA_ * scale; 00262 else 00263 return depth_focal_length_SXGA_ * scale; 00264 } 00265 00266 float OpenNIDevice::getBaseline () const throw () 00267 { 00268 return baseline_; 00269 } 00270 00271 template<typename T> OpenNIDevice::CallbackHandle OpenNIDevice::registerImageCallback (void (T::*callback)(boost::shared_ptr<Image>, void* cookie), T& instance, void* custom_data) throw () 00272 { 00273 image_callback_[image_callback_handle_counter_] = boost::bind (callback, boost::ref (instance), _1, custom_data); 00274 return image_callback_handle_counter_++; 00275 } 00276 00277 template<typename T> OpenNIDevice::CallbackHandle OpenNIDevice::registerDepthCallback (void (T::*callback)(boost::shared_ptr<DepthImage>, void* cookie), T& instance, void* custom_data) throw () 00278 { 00279 depth_callback_[depth_callback_handle_counter_] = boost::bind ( callback, boost::ref (instance), _1, custom_data); 00280 return depth_callback_handle_counter_++; 00281 } 00282 00283 template<typename T> OpenNIDevice::CallbackHandle OpenNIDevice::registerIRCallback (void (T::*callback)(boost::shared_ptr<IRImage>, void* cookie), T& instance, void* custom_data) throw () 00284 { 00285 ir_callback_[ir_callback_handle_counter_] = boost::bind ( callback, boost::ref (instance), _1, custom_data); 00286 return ir_callback_handle_counter_++; 00287 } 00288 00289 } 00290 #endif // __OPENNI_IDEVICE_H__ 00291 #endif // HAVE_OPENNI