openni_device.h
Go to the documentation of this file.
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 __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/function.hpp>
00049 #include <boost/thread.hpp>
00050 #include <boost/thread/condition.hpp>
00051 
00052 #include <pcl/pcl_macros.h>
00053 
00055 
00056 #ifndef _WIN32
00057 #define __stdcall
00058 #endif
00059 
00060 namespace openni_wrapper
00061 {
00062   class Image;
00063   class DepthImage;
00064   class IRImage;
00065 
00070   class PCL_EXPORTS OpenNIDevice
00071   {
00072   public:
00073     typedef boost::function<void(boost::shared_ptr<Image>, void* cookie) > ImageCallbackFunction;
00074     typedef boost::function<void(boost::shared_ptr<DepthImage>, void* cookie) > DepthImageCallbackFunction;
00075     typedef boost::function<void(boost::shared_ptr<IRImage>, void* cookie) > IRImageCallbackFunction;
00076     typedef unsigned CallbackHandle;
00077 
00078   public:
00079 
00081     virtual ~OpenNIDevice () throw ();
00082 
00090     bool 
00091     findCompatibleImageMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw ();
00092 
00100     bool 
00101     findCompatibleDepthMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw ();
00102 
00107     bool 
00108     isImageModeSupported (const XnMapOutputMode& output_mode) const throw ();
00109 
00114     bool 
00115     isDepthModeSupported (const XnMapOutputMode& output_mode) const throw ();
00116 
00121     const XnMapOutputMode& 
00122     getDefaultImageMode () const throw ();
00123 
00127     const XnMapOutputMode& 
00128     getDefaultDepthMode () const throw ();
00129 
00133     const XnMapOutputMode& 
00134     getDefaultIRMode () const throw ();
00135 
00139     void 
00140     setImageOutputMode (const XnMapOutputMode& output_mode);
00141 
00145     void 
00146     setDepthOutputMode (const XnMapOutputMode& output_mode);
00147 
00151     void 
00152     setIROutputMode (const XnMapOutputMode& output_mode);
00153 
00155     XnMapOutputMode 
00156     getImageOutputMode () const;
00157 
00159     XnMapOutputMode 
00160     getDepthOutputMode () const;
00161 
00163     XnMapOutputMode 
00164     getIROutputMode () const;
00165 
00169     void 
00170     setDepthRegistration (bool on_off);
00171 
00173     bool 
00174     isDepthRegistered () const throw ();
00175 
00177     bool 
00178     isDepthRegistrationSupported () const throw ();
00179 
00183     void 
00184     setSynchronization (bool on_off);
00185 
00187     bool 
00188     isSynchronized () const throw ();
00189 
00191     bool 
00192     isSynchronizationSupported () const throw ();
00193 
00195     bool 
00196     isDepthCropped () const;
00197 
00204     void 
00205     setDepthCropping (unsigned x, unsigned y, unsigned width, unsigned height);
00206 
00208     bool 
00209     isDepthCroppingSupported () const throw ();
00210 
00214     inline float 
00215     getImageFocalLength (int output_x_resolution = 0) const throw ();
00216 
00220     inline float 
00221     getDepthFocalLength (int output_x_resolution = 0) const throw ();
00222 
00224     inline float 
00225     getBaseline () const throw ();
00226 
00228     virtual void 
00229     startImageStream ();
00230 
00232     virtual void 
00233     stopImageStream ();
00234 
00236     virtual void 
00237     startDepthStream ();
00238 
00240     virtual void 
00241     stopDepthStream ();
00242 
00244     virtual void 
00245     startIRStream ();
00246 
00248     virtual void 
00249     stopIRStream ();
00250 
00252     bool 
00253     hasImageStream () const throw ();
00254 
00256     bool 
00257     hasDepthStream () const throw ();
00258 
00260     bool 
00261     hasIRStream () const throw ();
00262 
00264     virtual bool 
00265     isImageStreamRunning () const throw ();
00266 
00268     virtual bool 
00269     isDepthStreamRunning () const throw ();
00270 
00272     virtual bool 
00273     isIRStreamRunning () const throw ();
00274 
00281     CallbackHandle 
00282     registerImageCallback (const ImageCallbackFunction& callback, void* cookie = NULL) throw ();
00283 
00291     template<typename T> CallbackHandle 
00292     registerImageCallback (void (T::*callback)(boost::shared_ptr<Image>, void* cookie), T& instance, void* cookie = NULL) throw ();
00293 
00298     bool 
00299     unregisterImageCallback (const CallbackHandle& callbackHandle) throw ();
00300 
00301 
00308     CallbackHandle 
00309     registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = NULL) throw ();
00310 
00318     template<typename T> CallbackHandle 
00319     registerDepthCallback (void (T::*callback)(boost::shared_ptr<DepthImage>, void* cookie), T& instance, void* cookie = NULL) throw ();
00320 
00325     bool 
00326     unregisterDepthCallback (const CallbackHandle& callbackHandle) throw ();
00327 
00334     CallbackHandle 
00335     registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = NULL) throw ();
00336 
00344     template<typename T> CallbackHandle 
00345     registerIRCallback (void (T::*callback)(boost::shared_ptr<IRImage>, void* cookie), T& instance, void* cookie = NULL) throw ();
00346 
00351     bool 
00352     unregisterIRCallback (const CallbackHandle& callbackHandle) throw ();
00353 
00357     const char* 
00358     getSerialNumber () const throw ();
00359 
00361     const char* 
00362     getConnectionString () const throw ();
00363 
00365     const char* 
00366     getVendorName () const throw ();
00367 
00369     const char* 
00370     getProductName () const throw ();
00371 
00373     unsigned short 
00374     getVendorID () const throw ();
00375 
00377     unsigned short 
00378     getProductID () const throw ();
00379 
00381     unsigned char  
00382     getBus () const throw ();
00383 
00385     unsigned char  
00386     getAddress () const throw ();
00387 
00391     inline void
00392     setRGBFocalLength (float focal_length)
00393     {
00394       rgb_focal_length_SXGA_ = focal_length;
00395     }
00396 
00400     inline void
00401     setDepthFocalLength (float focal_length)
00402     {
00403       depth_focal_length_SXGA_ = focal_length;
00404     }
00405   private:
00406     // make OpenNIDevice non copyable
00407     OpenNIDevice (OpenNIDevice const &);
00408     OpenNIDevice& operator=(OpenNIDevice const &);
00409   protected:
00410     typedef boost::function<void(boost::shared_ptr<Image>) > ActualImageCallbackFunction;
00411     typedef boost::function<void(boost::shared_ptr<DepthImage>) > ActualDepthImageCallbackFunction;
00412     typedef boost::function<void(boost::shared_ptr<IRImage>) > ActualIRImageCallbackFunction;
00413 
00414     OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
00415     OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
00416     OpenNIDevice (xn::Context& context);
00417     static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
00418     static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
00419     static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
00420 
00421     // This is a workaround, since in the NewDepthDataAvailable function WaitAndUpdateData leads to a dead-lock behaviour
00422     // and retrieving image data without WaitAndUpdateData leads to incomplete images!!!
00423     void 
00424     ImageDataThreadFunction ();
00425 
00426     void 
00427     DepthDataThreadFunction ();
00428 
00429     void 
00430     IRDataThreadFunction ();
00431 
00432     virtual bool 
00433     isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const  throw () = 0;
00434 
00435     void 
00436     setRegistration (bool on_off);
00437 
00438     virtual boost::shared_ptr<Image> 
00439     getCurrentImage (boost::shared_ptr<xn::ImageMetaData> image_data) const throw () = 0;
00440 
00441     void 
00442     Init ();
00443     // holds the callback functions together with custom data
00444     // since same callback function can be registered multiple times with e.g. different custom data
00445     // we use a map structure with a handle as the key
00446     std::map<CallbackHandle, ActualImageCallbackFunction> image_callback_;
00447     std::map<CallbackHandle, ActualDepthImageCallbackFunction> depth_callback_;
00448     std::map<CallbackHandle, ActualIRImageCallbackFunction> ir_callback_;
00449 
00450     std::vector<XnMapOutputMode> available_image_modes_;
00451     std::vector<XnMapOutputMode> available_depth_modes_;
00452 
00454     xn::Context& context_;
00456     xn::NodeInfo device_node_info_;
00457 
00459     xn::DepthGenerator depth_generator_;
00461     xn::ImageGenerator image_generator_;
00463     xn::IRGenerator ir_generator_;
00464 
00465     XnCallbackHandle depth_callback_handle_;
00466     XnCallbackHandle image_callback_handle_;
00467     XnCallbackHandle ir_callback_handle_;
00468 
00470     float depth_focal_length_SXGA_;
00472     float baseline_;
00474     float rgb_focal_length_SXGA_;
00475 
00477     XnUInt64 shadow_value_;
00479     XnUInt64 no_sample_value_;
00480 
00481     OpenNIDevice::CallbackHandle image_callback_handle_counter_;
00482     OpenNIDevice::CallbackHandle depth_callback_handle_counter_;
00483     OpenNIDevice::CallbackHandle ir_callback_handle_counter_;
00484 
00485     bool quit_;
00486     mutable boost::mutex image_mutex_;
00487     mutable boost::mutex depth_mutex_;
00488     mutable boost::mutex ir_mutex_;
00489     boost::condition_variable image_condition_;
00490     boost::condition_variable depth_condition_;
00491     boost::condition_variable ir_condition_;
00492     boost::thread image_thread_;
00493     boost::thread depth_thread_;
00494     boost::thread ir_thread_;
00495   } ;
00496 
00498   float
00499   OpenNIDevice::getImageFocalLength (int output_x_resolution) const throw ()
00500   {
00501     if (output_x_resolution == 0)
00502       output_x_resolution = getImageOutputMode ().nXRes;
00503 
00504     float scale = static_cast<float> (output_x_resolution) / static_cast<float> (XN_SXGA_X_RES);
00505     return (rgb_focal_length_SXGA_ * scale);
00506   }
00507 
00509   float
00510   OpenNIDevice::getDepthFocalLength (int output_x_resolution) const throw ()
00511   {
00512     if (output_x_resolution == 0)
00513       output_x_resolution = getDepthOutputMode ().nXRes;
00514 
00515     float scale = static_cast<float> (output_x_resolution) / static_cast<float> (XN_SXGA_X_RES);
00516     if (isDepthRegistered ())
00517       return (rgb_focal_length_SXGA_ * scale);
00518     else
00519       return (depth_focal_length_SXGA_ * scale);
00520   }
00521 
00523   float
00524   OpenNIDevice::getBaseline () const throw ()
00525   {
00526     return (baseline_);
00527   }
00528 
00530   template<typename T> OpenNIDevice::CallbackHandle
00531   OpenNIDevice::registerImageCallback (void (T::*callback)(boost::shared_ptr<Image>, void* cookie), T& instance, void* custom_data) throw ()
00532   {
00533     image_callback_[image_callback_handle_counter_] = boost::bind (callback, boost::ref (instance), _1, custom_data);
00534     return (image_callback_handle_counter_++);
00535   }
00536 
00538   template<typename T> OpenNIDevice::CallbackHandle
00539   OpenNIDevice::registerDepthCallback (void (T::*callback)(boost::shared_ptr<DepthImage>, void* cookie), T& instance, void* custom_data) throw ()
00540   {
00541     depth_callback_[depth_callback_handle_counter_] = boost::bind ( callback,  boost::ref (instance), _1, custom_data);
00542     return (depth_callback_handle_counter_++);
00543   }
00544 
00546   template<typename T> OpenNIDevice::CallbackHandle
00547   OpenNIDevice::registerIRCallback (void (T::*callback)(boost::shared_ptr<IRImage>, void* cookie), T& instance, void* custom_data) throw ()
00548   {
00549     ir_callback_[ir_callback_handle_counter_] = boost::bind ( callback,  boost::ref (instance), _1, custom_data);
00550     return (ir_callback_handle_counter_++);
00551   }
00552 
00553 }
00554 #endif // __OPENNI_IDEVICE_H__
00555 #endif // HAVE_OPENNI


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:00