VideoSource_Kinect.hh

/* 
 This file is part of the BIAS library (Basic ImageAlgorithmS).

 Copyright (C) 2003, 2004    (see file CONTACTS for details)
 Multimediale Systeme der Informationsverarbeitung
 Institut fuer Informatik
 Christian-Albrechts-Universitaet Kiel


 BIAS is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 BIAS is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with BIAS; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#ifndef VIDEOSOURCE_KINECT_HH_
#define VIDEOSOURCE_KINECT_HH_

// freenect c++ interface header
#ifndef WIN32
  #include <libfreenect.hpp>
#else
  #include <libfreenect.h>
#endif
#include <VideoSource/VideoSource_Kinect_Base.hh>
#include <Image/Camera.hh>

namespace BIAS {
#ifndef WIN32
  class VideoSource_Kinect_Callback;
#endif
  /**
   * @brief Use Kinect with the libfreenect drivers just like the ToF Cameras. 
   * There is no parameters to control the camera yet.
   * Functionally might be added later depending on evolution of libfreenect
   * @note for usage in Linux mind, that you need write access to the USB port.
   * This can be granted by creating a udev rules file, e.g. 51-kinect.rules
   * with the following content:
            # ATTR{product}=="Xbox NUI Motor"
            SUBSYSTEM=="usb", ATTR{idVendor}=="045e", ATTR{idProduct}=="02b0", MODE="0666"

            # ATTR{product}=="Xbox NUI Audio"
            SUBSYSTEM=="usb", ATTR{idVendor}=="045e", ATTR{idProduct}=="02ad", MODE="0666"

            # ATTR{product}=="Xbox NUI Camera"
            SUBSYSTEM=="usb", ATTR{idVendor}=="045e", ATTR{idProduct}=="02ae", MODE="0666"
   * \ingroup g_videosource
   * @author fkellner 02/11
   */
  class BIASVideoSource_EXPORT VideoSource_Kinect : public VideoSource_Kinect_Base {
    public:
      /** \brief Contrructor */
      VideoSource_Kinect();
      /** \brief Destructor */
      virtual ~VideoSource_Kinect();
      
      /** \brief Opens first device found, OpenDevice(0) */
      virtual int OpenDevice();
      /** \brief Opens device by id. On first call inits the context and looks for available cameras (i.e. scan bus)  */
      virtual int OpenDevice(int device);

      /** \brief Close device, if no other kinect cameras are in use, context is deleted (i.e. close usb bus connection) */
      virtual int CloseDevice();

      /** \brief set appropriate operation mode for kinect and start grabbing thread 
       (see callbacks in VideoSource_Kinect_Callback)  */
      virtual int PreGrab();
      /** \brief stop grabbing thread  */
      virtual int PostGrab();
      
      /** \brief kinect color image will be 640x480, RGB.  */
      virtual int InitImage(BIAS::ImageBase &Image);
      /** \brief kinect depth image will be 640x480, float in Millimeters  */
      virtual int InitDepthImage(BIAS::ImageBase &Image);

      /** \brief grab single color image  */
      virtual int GrabSingle(BIAS::Camera <unsigned char> &image);
      /** \brief depth according to formula  by Stephane Magnenat, 
      see http://openkinect.org/wiki/Imaging_Information, todo: needs validation!  */
      virtual int GrabSingleDepth(BIAS::Camera <float> &image);

      virtual int CreateDepthLookupTable(double px=328.82739, double py=252.6967098, double fx=585.9303337, double fy=585.5482334);

      /**\brief Set the video mode to color*/
      virtual int SetVideoModeColor();
      /**\brief Set the video mode to infrared*/
      virtual int SetVideoModeIR();

      /**\brief set the depth capture mode
       * \param mode[in]: the capture mode */
      virtual int SetDepthCaptureMode(CaptureMode mode);
      /**\brief Set whether to switch between infrared and color after each grab */
      virtual int SetSwitchVideoModeAfterGrab();

      /**\brief Set the tilt angle of the Kinect Motor
       * \param degree[in]: the degrees to tilt*/
      virtual int SetTilt(double degrees);
      /**\brief Set the Led mode, e.g. blinking 
       * \param mode[in]: the mode of the led, see freenect_led_options*/
      virtual int SetLed(int mode);


      /**\brief Get the values of tilt and accelerometer
       * \param tilt[out]: the tilt angle
       * \param x[out]: x acceleration
       * \param y[out]: y acceleration
       * \param z[out]: z acceleration*/
      virtual int GetAccelerometer(double &tilt, double &x, double &y, double &z);

      virtual int GetCapabilities(BIAS::VideoSourceCapabilities &caps);
      virtual int GetCapabilities(const char *device, BIAS::VideoSourceCapabilities &caps);

      /** \brief Get the number of devices */
      virtual int GetNumDevices() {return deviceCount_; }
      
      virtual int ToggleDepthStream();

#ifdef WIN32
      void DeleteDepthLookupTable();
#endif

    private:
#ifndef WIN32
      /// freenect "context", quite similar to dc_bus in libdc1394 this wraps up all devices on the bus
      static Freenect::Freenect *kinectManager_;
      /// this instance's actual device
      VideoSource_Kinect_Callback *kinectDevice_;
#else
     
#endif
      /// total number of devices found on bus
      static int deviceCount_;
      /// number of open devices. when this reaches zero, context is closed
      static int openDevices_;
      /// this instance's device id. First cam found has id=0, second has id=1 etc
      int deviceId_;
  };

#ifndef WIN32
  /**
   * Implementing FreenectDevice Interface, this class is used *internally* by 
   * VideoSource_Kinect - do not use directly
   * @author fkellner 02/11
   */
  class VideoSource_Kinect_Callback : public Freenect::FreenectDevice
  {
    public:
      /**
       * libfreenect constructor used by VideoSource_Kinect, do not use directly
       * @param _ctx libfreenect context, i.e. opened usb bus
       * @param _index index of camera on context/bus
       * @author fkellner 02/11
       */
      VideoSource_Kinect_Callback(freenect_context *_ctx, int _index);

      /**
       * libfreenect thread calls this when new color data is available
       * @param video pointer to color bytes (3 per pixel), todo: convince libfreenect to deliver raw rggb data
       * @param timestamp frame timestamp, todo: interpret, put as metadata
       * @author fkellner 02/11
       */
      virtual void VideoCallback(void *video, uint32_t timestamp);

      /**
       * libfreenect thread calls this when new depth data is available
       * @param video pointer to depth bytes (2 per pixel)
       * @param timestamp frame timestamp, todo: interpret, put as metadata
       * @author fkellner 02/11
       */
      virtual void DepthCallback(void *depth, uint32_t timestamp);

      /**
       * Callbacks store image data in double buffer, get latest
       * @param image color image
       * @author fkellner 02/11
       */
      virtual int GetLatestColor(Camera<unsigned char> &image);

      /**
       * Callbacks store image data in double buffer, get latest
       * Note that the depth image is shifted by 3 pixels in order to fit the IR image.
       * @param image depth image
       * @author fkellner 02/11
       */
      virtual int GetLatestDepth(Camera<float> &image);
      
      virtual int SetVideoMode(freenect_video_format format);
      virtual int SetSwitchVideoModeAfterGrab(bool switchMode);
      virtual void SetDepthMode(int mode);
      
      void CreateLookupTable(double px, double py, double fx, double fy);

    protected:
      // double buffered cam data
      std::vector< Camera<unsigned char> > colorbuffer_; 
      std::vector< Camera<float> > depthbuffer_; 
      // indices into buffers
      int writingToC_;
      int writingToD_;
      
      // todo: clean up this mess, we do not need so many mutexes (fkellner)
      pthread_mutex_t *color_mutex;
      pthread_mutex_t *depth_mutex;
      pthread_cond_t *newColorAvail_;
      pthread_cond_t *newDepthAvail_;
      pthread_mutex_t *coloravail_mutex;
      pthread_mutex_t *depthavail_mutex;
      
      // lookup table for depth bytes -> mm conversion,
      // formula by Stephane Magnenat, see http://openkinect.org/wiki/Imaging_Information 
      float lookup_[2048];

      // lookup table for z-depth to ray conversion (calibrated using one specific kinect camera)
      Image<float> depthLookupTable_;

      // current color format
      freenect_video_format format_;

      bool switch_;      
      int capMode_;
  };
#endif
}

#endif /* VIDEOSOURCE_KINECT_HH_ */