VideoSource_Kinect_win32.cpp

/* 
 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
 */

#include "VideoSource_Kinect.hh"
#include <iostream>
#include <Base/Image/ImageIO.hh>
#include <pthread.h>

using namespace std;

namespace BIAS {

  /////////////////////////////////////////////////////////////////////////
  ///////////////////////VideoSource_Kinect////////////////////////////////
  /////////////////////////////////////////////////////////////////////////
 
  int VideoSource_Kinect::deviceCount_ = 0;
  int VideoSource_Kinect::openDevices_ = 0;
  freenect_device* freenectDevice_ = NULL;
  freenect_context* freenectContext_ = NULL;

  pthread_t freenect_thread;
  volatile int die = 0;
  
  // back: owned by libfreenect (implicit for depth)
  // mid: owned by callbacks, "latest frame ready"
  // front: owned by class, 
  float *depth_mid, *depth_front;
  unsigned char *rgb_back, *rgb_mid, *rgb_front;
  float lookup_[2048];

  freenect_video_format requested_format = FREENECT_VIDEO_BAYER;
  freenect_video_format current_format = FREENECT_VIDEO_BAYER;

  pthread_mutex_t gl_backbuf_mutex = PTHREAD_MUTEX_INITIALIZER;
  pthread_cond_t gl_frame_cond = PTHREAD_COND_INITIALIZER;
  
  pthread_mutex_t coloravail_mutex = PTHREAD_MUTEX_INITIALIZER;
  pthread_mutex_t depthavail_mutex = PTHREAD_MUTEX_INITIALIZER;
  pthread_cond_t newColorAvail_ = PTHREAD_COND_INITIALIZER;
  pthread_cond_t newDepthAvail_ = PTHREAD_COND_INITIALIZER; 

  int got_rgb = 0;
  int got_depth = 0;
  BIAS::Image<float> *depthLookupTable_;

  bool switch_;      
  int capMode_;

  VideoSource_Kinect::VideoSource_Kinect()
  {
    Active_ = false;
    Width_ = 640;
    Height_ = 480;
    capMode_ = (int)VideoSource_Kinect::DepthRay;
  }


  VideoSource_Kinect::~VideoSource_Kinect()
  {
    if (openDevices_ != 0) {
      CloseDevice();
    }
    DeleteDepthLookupTable();
  }

  int VideoSource_Kinect::
  OpenDevice() 
  {
    return OpenDevice(0);
  }

  int VideoSource_Kinect::
  OpenDevice(int device)
  {
    if (openDevices_ > 0 && device==0) {
      BIASWARN("Kinect: I guess you try to open the next device (device=0 given)");
      device = openDevices_;
      BIASWARN("Kinect: Trying to open device " << device << " instead");
    }

    if (freenect_init(&freenectContext_, NULL) < 0)
    {     
        BIASERR("Could not init freenect!");
      return -1;
      }
    
      deviceCount_ = freenect_num_devices (freenectContext_);
      cout<<"Number of devices found: "<< deviceCount_;

    if (deviceCount_ < 1){
          BIASERR("No devices found, returning");
      return -2;
    }

      if (freenect_open_device(freenectContext_, &freenectDevice_, device) < 0) {
          BIASERR("Could not open device.");
          return -3;
      }
    
    CreateDepthLookupTable();
 
    deviceId_ = device;
    openDevices_++;
    return 0;
  }
  
  int VideoSource_Kinect::CloseDevice() {
    openDevices_--;
    Active_ = false;
    return 0;
  }

  /////////////////////////////////////////////////////////////////////////////
  void DepthCallBack(freenect_device *dev, void *v_depth, uint32_t timestamp)
  {   
      uint16_t *depth = (uint16_t*)v_depth;
    float *lut = depthLookupTable_->GetImageData();

      pthread_mutex_lock(&gl_backbuf_mutex);
      switch (capMode_) {
      case VideoSource_Kinect::DepthRay: {
        for (unsigned int x=0;x<640*480;x++) {
          depth_mid[x] = lookup_[ depth[x] ] * lut[x];
        }
        break;
      }
      case VideoSource_Kinect::DepthEuclidean: {
        for (unsigned int x=0;x<640*480;x++) {
          depth_mid[x] = lookup_[ depth[x] ];
        }
        break;
      }
      case VideoSource_Kinect::DepthRaw: {
        for (unsigned int x=0;x<640*480;x++) {
          depth_mid[x] = lookup_[x];
        }
        break;
      }
      default: {
        BIASERR("unknown mode")
        break;
      }
    }
      got_depth++;
      pthread_cond_signal(&gl_frame_cond);
      pthread_mutex_unlock(&gl_backbuf_mutex);
    pthread_cond_signal(&newDepthAvail_);
  }
  /////////////////////////////////////////////////////////////////////////////
  void VideoCallback(freenect_device *dev, void *rgb, uint32_t timestamp)
  {
      pthread_mutex_lock(&gl_backbuf_mutex);

      // swap buffers
      assert (rgb_back == rgb);
      rgb_back = rgb_mid;
      freenect_set_video_buffer(dev, rgb_back);
      rgb_mid = (uint8_t*)rgb;

      got_rgb++;
      pthread_cond_signal(&gl_frame_cond);
      pthread_mutex_unlock(&gl_backbuf_mutex);
    pthread_cond_signal(&newColorAvail_);
  }

  /////////////////////////////////////////////////////////////////////////////
  void *FreenectThreadFunc_(void *arg)
  {
      freenect_set_tilt_degs(freenectDevice_,0);
      freenect_set_led(freenectDevice_,LED_RED);
      freenect_set_depth_callback(freenectDevice_, DepthCallBack);
      freenect_set_video_callback(freenectDevice_, VideoCallback);
      freenect_set_video_format(freenectDevice_, current_format);
      freenect_set_depth_format(freenectDevice_, FREENECT_DEPTH_11BIT);
      freenect_set_video_buffer(freenectDevice_, rgb_back);

      freenect_start_depth(freenectDevice_);
      freenect_start_video(freenectDevice_);

      while (!die && freenect_process_events(freenectContext_) >= 0)
    {   
          if (requested_format != current_format) {
              freenect_stop_video(freenectDevice_);
              freenect_set_video_format(freenectDevice_, requested_format);
              freenect_start_video(freenectDevice_);
              current_format = requested_format;
          }
      }

      freenect_stop_depth(freenectDevice_);
      freenect_stop_video(freenectDevice_);

      freenect_close_device(freenectDevice_);
      freenect_shutdown(freenectContext_);

      return NULL;
  }
////////////////////////////////////////////////////////////////////////
  
  int VideoSource_Kinect::PreGrab() {
    cout << "Pregrab Device " << deviceId_ << endl;
    int res = pthread_create(&freenect_thread, NULL, FreenectThreadFunc_, NULL);
      if (res) {
          BIASERR("Creating Freenect thread failed");
          return 1;
      }

    depth_mid = new float[640*480*3];
      depth_front = new float[640*480*3];
      rgb_back = new unsigned char[640*480*3];
      rgb_mid = new unsigned char[640*480*3];
      rgb_front = new unsigned char[640*480*3];

    for (int i=0; i<2048; i++) {
      const float k1 = 1.1863f;
      const float k2 = 2842.5f;
      const float k3 = 0.1236f;
      const float depth = k3 * tanf(i/k2 + k1);
      lookup_[i] = depth * 1000.0f;
    }
    lookup_[2047] = 0.0f;

    Active_ = true;
    return 0;
  }

  int VideoSource_Kinect::PostGrab() {
    die = 1;
        pthread_join(freenect_thread, NULL);
        delete[] depth_mid;
        delete[] depth_front;
        delete[] rgb_back;
        delete[] rgb_mid;
        delete[] rgb_front;     
    return 0;
  }


  int VideoSource_Kinect::
  InitImage(BIAS::ImageBase &Image)
  {
    if (!Image.IsEmpty()) {
      BIASWARN("Image already inited");
      return 1;
    }
    Image.Init(640,480,1);
    Image.SetColorModel(ImageBase::CM_Bayer_GRBG);
    return 0;
  }
  
  int VideoSource_Kinect::
  InitDepthImage(BIAS::ImageBase &Image)
  {
    if (!Image.IsEmpty()) {
      BIASWARN("Kinect Error: image already inited");
      return 1;
    }
    Image.Init(640,480,1,ImageBase::ST_float);
    Image.SetColorModel(ImageBase::CM_Depth);

    return 0;
  }
  
  int VideoSource_Kinect::
  GrabSingle(BIAS::Camera <unsigned char> &image)
  {
    pthread_cond_wait(&newColorAvail_, &coloravail_mutex);
    unsigned char *tmp;
      if (got_rgb) {
          tmp = rgb_front;
          rgb_front = rgb_mid;
          rgb_mid = tmp;
          got_rgb = 0;
      }
    image.CopyIn_NoInit(rgb_front);
    return 0;
  }

  int VideoSource_Kinect::GrabSingleDepth(BIAS::Camera <float> &image)
  {
    pthread_cond_wait(&newDepthAvail_, &depthavail_mutex);
    float *tmp;

      if (got_depth) {
          tmp = depth_front;
          depth_front = depth_mid;
          depth_mid = tmp;
          got_depth = 0;
      }
    image.CopyIn_NoInit(depth_front);
    return 0;
  }
  
  int VideoSource_Kinect::SetVideoModeColor() {
    requested_format = FREENECT_VIDEO_BAYER;
    return 0;
  }

  int VideoSource_Kinect::SetVideoModeIR() {
    requested_format = FREENECT_VIDEO_IR_8BIT;
    return 0;
  }
  
  int VideoSource_Kinect::SetDepthCaptureMode(CaptureMode mode) {
    capMode_ = (int)mode;
    return 0;
  }

  int VideoSource_Kinect::SetSwitchVideoModeAfterGrab() {
    BIASERR("Not implemented!");
    return 0;
  }
  
  int VideoSource_Kinect::SetTilt(double degrees) {
    freenect_set_tilt_degs(freenectDevice_,degrees);
    return 0;
  }

  int VideoSource_Kinect::SetLed(int mode) {
    freenect_set_led(freenectDevice_,(freenect_led_options)mode);
    return 0;
  }
  
  int VideoSource_Kinect::GetAccelerometer(double &tilt, double &x, double &y, double &z) {
    freenect_raw_tilt_state* state=NULL;
        freenect_update_tilt_state(freenectDevice_);
        state = freenect_get_tilt_state(freenectDevice_);
    tilt = freenect_get_tilt_degs(state);       
        freenect_get_mks_accel(state, &x, &y, &z);
    return 0;
  }

  int VideoSource_Kinect::GetCapabilities(VideoSourceCapabilities &caps)
  {
    return 0;
  }

  int VideoSource_Kinect::GetCapabilities(const char *device, VideoSourceCapabilities &caps)
  {
    return 0;
  }

  int VideoSource_Kinect::
  CreateDepthLookupTable(double px, double py, double fx, double fy)
  {
    if (depthLookupTable_ == NULL) {
      
      depthLookupTable_ = new Image<float>(640,480,1);
      float **ida = depthLookupTable_->GetImageDataArray();
      
      for(unsigned y=0;y<480;y++){
        for(unsigned x=0;x<640;x++){
          float alpha = atan(float((x-px)/fx));
          float beta = atan(float((py-y)/ sqrt( pow((x-px)*(fy/fx),2) + pow(fy,2))));
          ida[y][x] = 1.0f / (cos(beta) * cos(alpha));
        }
      }
    }
    return 0;
  }

  void VideoSource_Kinect::
  DeleteDepthLookupTable()
  {
    if (depthLookupTable_ != NULL) {
      delete depthLookupTable_;
      depthLookupTable_ = NULL;
    }
  }


  int VideoSource_Kinect::
  ToggleDepthStream(){
   BIASERR("ToggleDepthStream not implemented for windows and libfreenect.");
   return -1;
  }
}