VideoSource_Kinect.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>

using namespace std;

namespace BIAS {

  /////////////////////////////////////////////////////////////////////////
  ///////////////////////VideoSource_Kinect////////////////////////////////
  /////////////////////////////////////////////////////////////////////////
  
  Freenect::Freenect *VideoSource_Kinect::kinectManager_ = NULL;
  int VideoSource_Kinect::deviceCount_ = 0;
  int VideoSource_Kinect::openDevices_ = 0;
  

  VideoSource_Kinect::VideoSource_Kinect() {
    kinectDevice_ = NULL;
    Active_ = false;
    Width_ = 640;
    Height_ = 480;
  }


  VideoSource_Kinect::~VideoSource_Kinect() {

    if (openDevices_ != 0) {
      CloseDevice();
    }
    if (openDevices_ == 0) {
      // todo this does not always return
      //if (kinectManager_ != NULL) delete kinectManager_;
      kinectManager_ = NULL;
    }
  }

  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 (kinectManager_ == NULL) {
      try {
        kinectManager_ = new Freenect::Freenect();
        deviceCount_ = kinectManager_->deviceCount();
      } catch (std::runtime_error e) {
        BIASERR("Kinect Error: " << e.what());
        return -1;
      }
    }
    try {
      kinectDevice_ = &kinectManager_->createDevice<VideoSource_Kinect_Callback>(device);
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return -2;
    }
    if (kinectDevice_ == NULL) {
      BIASERR("Kinect Error: Device can not be opened");
      return -3;
    }
    CreateDepthLookupTable();
    deviceId_ = device;
    openDevices_++;
    return 0;
  }
  
  int VideoSource_Kinect::CloseDevice() {
    kinectManager_->deleteDevice(deviceId_);
    openDevices_--;
    kinectDevice_ = NULL;
    Active_ = false;
    return 0;
  }
  
  int VideoSource_Kinect::PreGrab() {
    cout << "pregrab device " << deviceId_ << endl;
    BIASASSERT(kinectDevice_ != NULL)
    try {
      kinectDevice_->setDepthFormat(FREENECT_DEPTH_11BIT);
      kinectDevice_->setVideoFormat(FREENECT_VIDEO_BAYER);
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return 1;
    }
    try {
      kinectDevice_->startDepth();
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return 1;
    }
    try { 
      kinectDevice_->startVideo(); // todo this causes error as setVideoFormat already starts video, ignore for now 
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      Active_ = true;
      return 0;
    }
    Active_ = true;
    return 0;
  }
  int VideoSource_Kinect::PostGrab() {
    BIASASSERT(kinectDevice_ != NULL)
    try {
      kinectDevice_->stopVideo();
      kinectDevice_->stopDepth();
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return 1;
    }
    return 0;
  }

  int VideoSource_Kinect::
  InitImage(BIAS::ImageBase &Image)
  {
    if (!Image.IsEmpty()) {
      BIASWARN("Kinect Error: 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) {
    BIASASSERT(kinectDevice_ != NULL)
    return kinectDevice_->GetLatestColor(image);
  }
  
  int VideoSource_Kinect::CreateDepthLookupTable(double px, double py, double fx, double fy) {
    BIASASSERT(kinectDevice_ != NULL)
    kinectDevice_->CreateLookupTable(px,py,fx,fy);
    return 0;
  }

  int VideoSource_Kinect::GrabSingleDepth(BIAS::Camera <float> &image) {
    BIASASSERT(kinectDevice_ != NULL)
    return kinectDevice_->GetLatestDepth(image);
  }
  
  int VideoSource_Kinect::SetVideoModeColor() {
    BIASASSERT(kinectDevice_ != NULL)
    kinectDevice_->SetSwitchVideoModeAfterGrab(false);
    return kinectDevice_->SetVideoMode(FREENECT_VIDEO_BAYER);
  }

  int VideoSource_Kinect::SetVideoModeIR() {
    BIASASSERT(kinectDevice_ != NULL)
    kinectDevice_->SetSwitchVideoModeAfterGrab(false);
    return kinectDevice_->SetVideoMode(FREENECT_VIDEO_IR_8BIT);
  }
  
  int VideoSource_Kinect::SetDepthCaptureMode(CaptureMode mode) {
    BIASASSERT(kinectDevice_ != NULL)
    kinectDevice_->SetDepthMode(mode);
    return 0;
  }

  int VideoSource_Kinect::SetSwitchVideoModeAfterGrab() {
    return kinectDevice_->SetSwitchVideoModeAfterGrab(true);
  }
  
  int VideoSource_Kinect::SetTilt(double degrees) {
    BIASASSERT(kinectDevice_ != NULL)
    try {
      if (degrees<-30.0) {
        BIASWARN("Kinect: clipping angle to -30");
        degrees = -30;
      }
      if (degrees>30.0) {
        BIASWARN("Kinect: clipping angle to 30");
        degrees = 30;
      }
      kinectDevice_->setTiltDegrees(degrees);
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return 1;
    }
    return 0;
  }

  int VideoSource_Kinect::SetLed(int mode) {
    BIASASSERT(kinectDevice_ != NULL)
    try {
      kinectDevice_->setLed((freenect_led_options)mode);
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return 1;
    }
    return 0;
  }
  
  int VideoSource_Kinect::GetAccelerometer(double &tilt, double &x, double &y, double &z) {
    BIASASSERT(kinectDevice_ != NULL)
    try {
      kinectDevice_->updateState();
      Freenect::FreenectTiltState state = kinectDevice_->getState();
      tilt = state.getTiltDegs();
      state.getAccelerometers(&x, &y, &z);
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return 1;
    }
    return 0;
  }

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

  int VideoSource_Kinect::GetCapabilities(const char *device, VideoSourceCapabilities &caps)
  {
    return 0;
  }
  
  int VideoSource_Kinect::ToggleDepthStream() {
    BIASERR("ToggleDepthStream not implemented for libfreenect");
    return 0;
  }

 ////////////////////////////////////////////////////////////////////////////////
  //////////////////VideoSource_Kinect_Callback //////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////

  VideoSource_Kinect_Callback::
  VideoSource_Kinect_Callback(freenect_context *_ctx, int _index) 
  : Freenect::FreenectDevice(_ctx, _index)
  {
    colorbuffer_.resize(2);
    colorbuffer_[0].Init(640,480,1);
    colorbuffer_[1].Init(640,480,1);
    depthbuffer_.resize(2);
    depthbuffer_[0].Init(640,480,1);
    depthbuffer_[1].Init(640,480,1);
    colorbuffer_[0].SetColorModel(ImageBase::CM_Bayer_GRBG);
    colorbuffer_[1].SetColorModel(ImageBase::CM_Bayer_GRBG);
    writingToC_ = 0;
    writingToD_ = 0;
    color_mutex = new pthread_mutex_t;
    depth_mutex = new pthread_mutex_t;
    coloravail_mutex = new pthread_mutex_t;
    depthavail_mutex = new pthread_mutex_t;
    newColorAvail_ = new pthread_cond_t;
    newDepthAvail_ = new pthread_cond_t;
    pthread_mutex_init(color_mutex, NULL);
    pthread_mutex_init(depth_mutex, NULL);
    pthread_mutex_init(coloravail_mutex, NULL);
    pthread_mutex_init(depthavail_mutex, NULL);
    pthread_cond_init(newColorAvail_, NULL);
    pthread_cond_init(newDepthAvail_, NULL);
    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;
    switch_ = false;
    capMode_ = (int)VideoSource_Kinect::DepthRay;
  }

  void VideoSource_Kinect_Callback::
  VideoCallback(void *video, uint32_t timestamp)
  {
    //cout << "color writing to buffer " << writingToC_ << endl;
    colorbuffer_[writingToC_].CopyIn_NoInit(video);
    //colorbuffer_[writingToC_].SetTime(timestamp, 0); // todo
    //cout << timestamp << endl;
    pthread_mutex_lock(color_mutex);
    writingToC_ = (writingToC_ + 1)%2;
    pthread_mutex_unlock(color_mutex);
    pthread_cond_signal(newColorAvail_);
  }

  void VideoSource_Kinect_Callback::
  DepthCallback(void *depth, uint32_t timestamp)
  {
    //cout << "depth writing to buffer " << writingToD_ << endl;
    //depth is 16-bit/ pixel, contains 11-bit data
#ifdef WIN32
    //unsigned short *pDepth = (unsigned short*)depth;
    uint16_t *pDepth = (uint16_t*)depth;
#else
    u_int16_t *pDepth = (u_int16_t*)depth;
#endif
    float *buf = depthbuffer_[writingToD_].GetImageData();
    float *lut = depthLookupTable_.GetImageData();
    switch (capMode_) {
      case VideoSource_Kinect::DepthRay: {
        for (unsigned int x=0;x<640*480;x++) {
          buf[x] = lookup_[ pDepth[x] ] * lut[x];
        }
        break;
      }
      case VideoSource_Kinect::DepthEuclidean: {
        for (unsigned int x=0;x<640*480;x++) {
          buf[x] = lookup_[ pDepth[x] ];
        }
        break;
      }
      case VideoSource_Kinect::DepthRaw: {
        for (unsigned int x=0;x<640*480;x++) {
          buf[x] = pDepth[x];
        }
        break;
      }
      default: {
        BIASERR("unknown mode")
        break;
      }
    }
    //depthbuffer_[writingToD_].SetTime(timestamp, 0); // todo
    pthread_mutex_lock(depth_mutex);
    writingToD_ = (writingToD_ + 1)%2;
    pthread_mutex_unlock(depth_mutex);
    pthread_cond_signal(newDepthAvail_);
  }

  int VideoSource_Kinect_Callback::
  GetLatestColor(Camera<unsigned char> &image)
  {
    pthread_cond_wait(newColorAvail_, coloravail_mutex);
    pthread_mutex_lock(color_mutex);
    int idx = (writingToC_ + 1)%2;
    if (switch_) {
      if (format_ == FREENECT_VIDEO_BAYER) {
        SetVideoMode(FREENECT_VIDEO_IR_8BIT);
      } else {
        SetVideoMode(FREENECT_VIDEO_BAYER);
      }
    }
    pthread_mutex_unlock(color_mutex);
    // cout << "grabbing " << idx << endl;
    image.CopyIn_NoInit(colorbuffer_[idx].GetImageData());
    return 0;
  }
  
  int VideoSource_Kinect_Callback::
  GetLatestDepth(Camera<float> &image)
  {
    pthread_cond_wait(newDepthAvail_, depthavail_mutex);
    pthread_mutex_lock(depth_mutex);
    int idx = (writingToD_ + 1)%2;
    pthread_mutex_unlock(depth_mutex);

    // Copy and compensate for the +3 pixel offset in the depth image
    float* idaDst = image.GetImageData();
    memset(idaDst,0,3*sizeof(float));
    memcpy(idaDst+3, depthbuffer_[idx].GetImageData(), (480*640-3)*sizeof(float));


    //image.CopyIn_NoInit(depthbuffer_[idx].GetImageData());
    return 0;
  }

  int VideoSource_Kinect_Callback::
  SetVideoMode(freenect_video_format format)
  {
    try {
      setVideoFormat(format);
      format_ = format;
    } catch (std::runtime_error e) {
      BIASERR("Kinect Error: " << e.what());
      return 1;
    }
    // todo already started by setformat??
    //    try { 
    //      startVideo();  
    //    } catch (std::runtime_error e) {
    //      BIASERR("Kinect Error: " << e.what());
    //      return 1;
    //    }
    return 0;
  }
  
  int VideoSource_Kinect_Callback::
  SetSwitchVideoModeAfterGrab(bool switchMode)
  {
    switch_ = switchMode;
    return 0;
  }
  
  void VideoSource_Kinect_Callback::
  SetDepthMode(int mode)
  {
    capMode_ = mode;
  }

  
  void VideoSource_Kinect_Callback::
  CreateLookupTable(double px, double py, double fx, double fy)
  {
    if (depthLookupTable_.IsEmpty()) {
      depthLookupTable_.Init(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));
      }
    }
  }

}//end namespace BIAS