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 Willow Garage, Inc. 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 <XnCppWrapper.h>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <boost/thread/condition.hpp>

#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 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 ();

    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;
    }
  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 ();
    // 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