openni_device.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2009-2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <pcl/pcl_config.h>
#ifdef HAVE_OPENNI

#ifndef __OPENNI_IDEVICE_H__
#define __OPENNI_IDEVICE_H__
#include <map>
#include <vector>
#include <utility>
#include "openni_exception.h"
#include "openni.h"

#include <pcl/io/boost.h>
#include <pcl/pcl_macros.h>



#ifndef _WIN32
#define __stdcall
#endif

namespace openni_wrapper
{
  class Image;
  class DepthImage;
  class IRImage;

  class PCL_EXPORTS OpenNIDevice
  {
    public:
      typedef enum
      {
        OpenNI_shift_values = 0, // Shift values (disparity)
        OpenNI_12_bit_depth = 1, // Default mode: regular 12-bit depth
      } DepthMode;

      typedef boost::function<void(boost::shared_ptr<Image>, void* cookie) > ImageCallbackFunction;
      typedef boost::function<void(boost::shared_ptr<DepthImage>, void* cookie) > DepthImageCallbackFunction;
      typedef boost::function<void(boost::shared_ptr<IRImage>, void* cookie) > IRImageCallbackFunction;
      typedef unsigned CallbackHandle;

    public:

      virtual ~OpenNIDevice () throw ();

      bool 
      findCompatibleImageMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw ();

      bool 
      findCompatibleDepthMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw ();

      bool 
      isImageModeSupported (const XnMapOutputMode& output_mode) const throw ();

      bool 
      isDepthModeSupported (const XnMapOutputMode& output_mode) const throw ();

      const XnMapOutputMode& 
      getDefaultImageMode () const throw ();

      const XnMapOutputMode& 
      getDefaultDepthMode () const throw ();

      const XnMapOutputMode& 
      getDefaultIRMode () const throw ();

      void 
      setImageOutputMode (const XnMapOutputMode& output_mode);

      void 
      setDepthOutputMode (const XnMapOutputMode& output_mode);

      void 
      setIROutputMode (const XnMapOutputMode& output_mode);

      XnMapOutputMode 
      getImageOutputMode () const;

      XnMapOutputMode 
      getDepthOutputMode () const;

      XnMapOutputMode 
      getIROutputMode () const;

      void 
      setDepthRegistration (bool on_off);

      bool 
      isDepthRegistered () const throw ();

      bool 
      isDepthRegistrationSupported () const throw ();

      void 
      setSynchronization (bool on_off);

      bool 
      isSynchronized () const throw ();

      virtual bool 
      isSynchronizationSupported () const throw ();

      bool 
      isDepthCropped () const;

      void 
      setDepthCropping (unsigned x, unsigned y, unsigned width, unsigned height);

      bool 
      isDepthCroppingSupported () const throw ();

      inline float 
      getImageFocalLength (int output_x_resolution = 0) const throw ();

      inline float 
      getDepthFocalLength (int output_x_resolution = 0) const throw ();

      inline float 
      getBaseline () const throw ();

      virtual void 
      startImageStream ();

      virtual void 
      stopImageStream ();

      virtual void 
      startDepthStream ();

      virtual void 
      stopDepthStream ();

      virtual void 
      startIRStream ();

      virtual void 
      stopIRStream ();

      bool 
      hasImageStream () const throw ();

      bool 
      hasDepthStream () const throw ();

      bool 
      hasIRStream () const throw ();

      virtual bool 
      isImageStreamRunning () const throw ();

      virtual bool 
      isDepthStreamRunning () const throw ();

      virtual bool 
      isIRStreamRunning () const throw ();

      CallbackHandle 
      registerImageCallback (const ImageCallbackFunction& callback, void* cookie = NULL) throw ();

      template<typename T> CallbackHandle 
      registerImageCallback (void (T::*callback)(boost::shared_ptr<Image>, void* cookie), T& instance, void* cookie = NULL) throw ();

      bool 
      unregisterImageCallback (const CallbackHandle& callbackHandle) throw ();


      CallbackHandle 
      registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = NULL) throw ();

      template<typename T> CallbackHandle 
      registerDepthCallback (void (T::*callback)(boost::shared_ptr<DepthImage>, void* cookie), T& instance, void* cookie = NULL) throw ();

      bool 
      unregisterDepthCallback (const CallbackHandle& callbackHandle) throw ();

      CallbackHandle 
      registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = NULL) throw ();

      template<typename T> CallbackHandle 
      registerIRCallback (void (T::*callback)(boost::shared_ptr<IRImage>, void* cookie), T& instance, void* cookie = NULL) throw ();

      bool 
      unregisterIRCallback (const CallbackHandle& callbackHandle) throw ();

      const char* 
      getSerialNumber () const throw ();

      const char* 
      getConnectionString () const throw ();

      const char* 
      getVendorName () const throw ();

      const char* 
      getProductName () const throw ();

      unsigned short 
      getVendorID () const throw ();

      unsigned short 
      getProductID () const throw ();

      unsigned char  
      getBus () const throw ();

      unsigned char  
      getAddress () const throw ();

      inline void
      setRGBFocalLength (float focal_length)
      {
        rgb_focal_length_SXGA_ = focal_length;
      }

      inline void
      setDepthFocalLength (float focal_length)
      {
        depth_focal_length_SXGA_ = focal_length;
      }

      void
      setDepthOutputFormat (const DepthMode& depth_mode = OpenNI_12_bit_depth);

      XnUInt64 
      getDepthOutputFormat () const;


      pcl::uint16_t
      shiftToDepth (pcl::uint16_t shift_value) const
      {
        assert (shift_conversion_parameters_.init_);

        pcl::uint16_t ret = 0;

        // lookup depth value in shift lookup table
        if (shift_value<shift_to_depth_table_.size())
          ret = shift_to_depth_table_[shift_value];

        return ret;
      }

    private:
      // make OpenNIDevice non copyable
      OpenNIDevice (OpenNIDevice const &);
      OpenNIDevice& operator=(OpenNIDevice const &);
    protected:
      typedef boost::function<void(boost::shared_ptr<Image>) > ActualImageCallbackFunction;
      typedef boost::function<void(boost::shared_ptr<DepthImage>) > ActualDepthImageCallbackFunction;
      typedef boost::function<void(boost::shared_ptr<IRImage>) > ActualIRImageCallbackFunction;

      OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
      OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
      OpenNIDevice (xn::Context& context);
      static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
      static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
      static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) throw ();

      // This is a workaround, since in the NewDepthDataAvailable function WaitAndUpdateData leads to a dead-lock behaviour
      // and retrieving image data without WaitAndUpdateData leads to incomplete images!!!
      void 
      ImageDataThreadFunction ();

      void 
      DepthDataThreadFunction ();

      void 
      IRDataThreadFunction ();

      virtual bool 
      isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const  throw () = 0;

      void 
      setRegistration (bool on_off);

      virtual boost::shared_ptr<Image> 
      getCurrentImage (boost::shared_ptr<xn::ImageMetaData> image_data) const throw () = 0;

      void 
      Init ();

      void InitShiftToDepthConversion();
      void ReadDeviceParametersFromSensorNode();

      struct ShiftConversion
      {
        ShiftConversion() : init_(false) {}

        XnUInt16 zero_plane_distance_;
        XnFloat zero_plane_pixel_size_;
        XnFloat emitter_dcmos_distace_;
        XnUInt32 max_shift_;
        XnUInt32 device_max_shift_;
        XnUInt32 const_shift_;
        XnUInt32 pixel_size_factor_;
        XnUInt32 param_coeff_;
        XnUInt32 shift_scale_;
        XnUInt32 min_depth_;
        XnUInt32 max_depth_;
        bool init_;

      } shift_conversion_parameters_;

      std::vector<pcl::uint16_t> shift_to_depth_table_;

      // holds the callback functions together with custom data
      // since same callback function can be registered multiple times with e.g. different custom data
      // we use a map structure with a handle as the key
      std::map<CallbackHandle, ActualImageCallbackFunction> image_callback_;
      std::map<CallbackHandle, ActualDepthImageCallbackFunction> depth_callback_;
      std::map<CallbackHandle, ActualIRImageCallbackFunction> ir_callback_;

      std::vector<XnMapOutputMode> available_image_modes_;
      std::vector<XnMapOutputMode> available_depth_modes_;

      xn::Context& context_;
      xn::NodeInfo device_node_info_;

      xn::DepthGenerator depth_generator_;
      xn::ImageGenerator image_generator_;
      xn::IRGenerator ir_generator_;

      XnCallbackHandle depth_callback_handle_;
      XnCallbackHandle image_callback_handle_;
      XnCallbackHandle ir_callback_handle_;

      float depth_focal_length_SXGA_;
      float baseline_;
      float rgb_focal_length_SXGA_;

      XnUInt64 shadow_value_;
      XnUInt64 no_sample_value_;

      OpenNIDevice::CallbackHandle image_callback_handle_counter_;
      OpenNIDevice::CallbackHandle depth_callback_handle_counter_;
      OpenNIDevice::CallbackHandle ir_callback_handle_counter_;

      bool quit_;
      mutable boost::mutex image_mutex_;
      mutable boost::mutex depth_mutex_;
      mutable boost::mutex ir_mutex_;
      boost::condition_variable image_condition_;
      boost::condition_variable depth_condition_;
      boost::condition_variable ir_condition_;
      boost::thread image_thread_;
      boost::thread depth_thread_;
      boost::thread ir_thread_;
  };

  float
  OpenNIDevice::getImageFocalLength (int output_x_resolution) const throw ()
  {
    if (output_x_resolution == 0)
      output_x_resolution = getImageOutputMode ().nXRes;

    float scale = static_cast<float> (output_x_resolution) / static_cast<float> (XN_SXGA_X_RES);
    return (rgb_focal_length_SXGA_ * scale);
  }

  float
  OpenNIDevice::getDepthFocalLength (int output_x_resolution) const throw ()
  {
    if (output_x_resolution == 0)
      output_x_resolution = getDepthOutputMode ().nXRes;

    float scale = static_cast<float> (output_x_resolution) / static_cast<float> (XN_SXGA_X_RES);
    if (isDepthRegistered ())
      return (rgb_focal_length_SXGA_ * scale);
    else
      return (depth_focal_length_SXGA_ * scale);
  }

  float
  OpenNIDevice::getBaseline () const throw ()
  {
    return (baseline_);
  }

  template<typename T> OpenNIDevice::CallbackHandle
  OpenNIDevice::registerImageCallback (void (T::*callback)(boost::shared_ptr<Image>, void* cookie), T& instance, void* custom_data) throw ()
  {
    image_callback_[image_callback_handle_counter_] = boost::bind (callback, boost::ref (instance), _1, custom_data);
    return (image_callback_handle_counter_++);
  }

  template<typename T> OpenNIDevice::CallbackHandle
  OpenNIDevice::registerDepthCallback (void (T::*callback)(boost::shared_ptr<DepthImage>, void* cookie), T& instance, void* custom_data) throw ()
  {
    depth_callback_[depth_callback_handle_counter_] = boost::bind ( callback,  boost::ref (instance), _1, custom_data);
    return (depth_callback_handle_counter_++);
  }

  template<typename T> OpenNIDevice::CallbackHandle
  OpenNIDevice::registerIRCallback (void (T::*callback)(boost::shared_ptr<IRImage>, void* cookie), T& instance, void* custom_data) throw ()
  {
    ir_callback_[ir_callback_handle_counter_] = boost::bind ( callback,  boost::ref (instance), _1, custom_data);
    return (ir_callback_handle_counter_++);
  }

}
#endif // __OPENNI_IDEVICE_H__
#endif // HAVE_OPENNI