VideoSource_OpenNI.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_OpenNI.hh"
#include <iostream>
#include <Base/Image/ImageIO.hh>

using namespace std;

namespace BIAS {

  int VideoSource_OpenNI::numInstances_ = 0;
  int VideoSource_OpenNI::numDevices_ = 0;
  xn::Context VideoSource_OpenNI::context_;
  std::vector<VideoSource_OpenNI::ONIDeviceNode> VideoSource_OpenNI::devices_; 

  xn::NodeInfoList VideoSource_OpenNI::device_nodes_;
  xn::NodeInfoList VideoSource_OpenNI::depth_nodes_;
  xn::NodeInfoList VideoSource_OpenNI::image_nodes_;
  xn::NodeInfoList VideoSource_OpenNI::ir_nodes_;

  pthread_mutex_t *VideoSource_OpenNI::exclusive_mutex = NULL;
  
  VideoSource_OpenNI::VideoSource_OpenNI() {
    Active_ = false;
    Width_ = 640;
    Height_ = 480;
    depthLookupTable_ = NULL;

    CreateLookupTable_();

    if (numDevices_ == 0) {
      ScanBus();
    }
    if (numInstances_ == 0) {
      exclusive_mutex = new pthread_mutex_t;
      pthread_mutex_init(exclusive_mutex, NULL);
    }
    numInstances_ ++;
    capMode_ = DepthRay;
    colorMode_ = false;
    contextInited_ = false;
    switchModeEveryFrame_ = false;
  }

  bool VideoSource_OpenNI::CheckError_(XnStatus rc, std::string what) {
    if (rc != XN_STATUS_OK)                     
    {                               
      BIASERR("OpenNI Fail: " << what << " : " << xnGetStatusString(rc));   
      return true;                          
    }
    return false;
  }


  VideoSource_OpenNI::~VideoSource_OpenNI() {
    numInstances_ --;
    if (numInstances_ == 0) {
      DeleteLookupTable_();
    }
    if (numInstances_ == 0) {
      context_.Shutdown();
      numDevices_ = 0;
      devices_.clear();
    }
  }

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

  int VideoSource_OpenNI::OpenDevice(int device) {
    deviceId_ = device;
    int ret = SetVideoModeColor();
    if (ret == 0) {
      return StartDepthStream();
    } else {
      return 1;
    }
  }
  
  int VideoSource_OpenNI::CloseDevice() {
    Active_ = false;
    return 0;
  }
  
  int VideoSource_OpenNI::PreGrab() {
    cout << "OpenNI: pregrab device" << endl;
    Active_ = true;
    return 0;
  }
  int VideoSource_OpenNI::PostGrab() {
    cout << "OpenNI: postgrab device " << endl;
    XnStatus rc;
    if (colorMode_ == false) {
      rc = irGenerator_.StopGenerating();
      if (CheckError_(rc, "stop generating IR")) return 1;
    } else {
      rc = colorGenerator_.StopGenerating();
      if (CheckError_(rc, "stop generating color")) return 1;
    }
    if (depthGenerator_.IsValid()) {
      rc = depthGenerator_.StopGenerating();
      if (CheckError_(rc, "stop generating depth")) return 1;
    }
    Active_ = false;
    return 0;
  }

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

    return 0;
  }
  
  int VideoSource_OpenNI::GrabSingle(BIAS::Camera <unsigned char> &image) {
    if (deviceId_ == 0) {
      pthread_mutex_lock(exclusive_mutex);
      context_.WaitAndUpdateAll();
      pthread_mutex_unlock(exclusive_mutex);
    }
//    context_.WaitOneUpdateAll(depthGenerator_);
//    if (colorMode_) {
//      context_.WaitOneUpdateAll(colorGenerator_);
//    } else {
//      context_.WaitOneUpdateAll(irGenerator_);
//    }
    if (depthGenerator_.IsValid())
      depthGenerator_.GetMetaData(depthMD_);
    if (colorMode_) {
      colorGenerator_.GetMetaData(colorMD_);
      image.CopyIn_NoInit( (void*)colorMD_.Data() );
    } else {
      irGenerator_.GetMetaData(irMD_);
      unsigned char *ida = image.GetImageData();
      const XnIRPixel *pix = irMD_.Data();
      for (int x=0;x<Width_*Height_;x++) {
        ida[x] = (unsigned char) (pix[x] / 2);
      }
    }
    if (switchModeEveryFrame_) {
      if (colorMode_ == true) {
        SetVideoModeColor();
      } else {
        SetVideoModeIR();
      }
    }
    return 0;
  }
  int VideoSource_OpenNI::GrabSingleDepth(BIAS::Camera <float> &image) {
    if (!depthGenerator_.IsValid()) {
      return 0;
    }
    const XnDepthPixel *data = depthMD_.Data();
    float *ida = image.GetImageData();
    
    switch (capMode_) {
      case DepthRay: {
        const float *lut = depthLookupTable_->GetImageData();
        memset(ida,0,3*sizeof(float));
        int size = Width_*Height_-3;
        for (int x=0;x<size;x++) {
           ida[x+3] = data[x] * lut[x];
        }
        break;
      }
      case DepthEuclidean: {
        memset(ida,0,3*sizeof(float));
        int size = Width_*Height_-3;
        for (int x=0;x<size;x++) {
                ida[x+3] = data[x];
        }

        break;
      }
      default:
        BIASERR("OpenNI capture mode not supported");
        break;
    }
    return 0;
  }
  
  int VideoSource_OpenNI::CreateDepthLookupTable(double px, double py, double fx, double fy) {
    pthread_mutex_lock(exclusive_mutex);
    CreateLookupTable_(px,py,fx,fy);
    pthread_mutex_unlock(exclusive_mutex);
    return 0;
  }
  
  int VideoSource_OpenNI::StartDepthStream() {
    pthread_mutex_lock(exclusive_mutex);
    if (depthGenerator_.IsValid()) {
      pthread_mutex_unlock(exclusive_mutex);
      return 1;
    }
    XnStatus rc;
    // create the production nodes
    rc = context_.CreateProductionTree (devices_[deviceId_].depthInfo);
    CheckError_(rc, "create depth generator");

    rc = devices_[deviceId_].depthInfo.GetInstance(depthGenerator_);
    CheckError_(rc, "create depth generator instance");

    XnMapOutputMode depth_mode;

    depth_mode.nXRes = XN_VGA_X_RES;
    depth_mode.nYRes = XN_VGA_Y_RES;
    depth_mode.nFPS = 30;

    rc = depthGenerator_.SetMapOutputMode(depth_mode);
    if (CheckError_(rc, "set depth mode")) return 1;

    rc = depthGenerator_.StartGenerating();
    if (CheckError_(rc, "starting depth cam")) return 1;

    pthread_mutex_unlock(exclusive_mutex);
    return 0;
  }

  int VideoSource_OpenNI::StopDepthStream() {
    pthread_mutex_lock(exclusive_mutex);
    if (depthGenerator_.IsValid() && depthGenerator_.IsGenerating()) {
      depthGenerator_.StopGenerating();
      depthGenerator_.Release();
    }
    pthread_mutex_unlock(exclusive_mutex);
    return 0;
  }
  
  int VideoSource_OpenNI::ToggleDepthStream() {
    if (depthGenerator_.IsValid()) {
      return StopDepthStream();
    } else {
      return StartDepthStream();
    }
  }

  int VideoSource_OpenNI::SetVideoModeColor() {
    pthread_mutex_lock(exclusive_mutex);
    if (colorMode_ == false) {
      if (irGenerator_.IsValid() && irGenerator_.IsGenerating()) {
        irGenerator_.StopGenerating();
        irGenerator_.Release();
      }
    } else {
      pthread_mutex_unlock(exclusive_mutex);
      return 0;
    }
    XnStatus rc;
    rc = context_.CreateProductionTree (devices_[deviceId_].imageInfo);
    CheckError_(rc, "create image generator");

    rc = devices_[deviceId_].imageInfo.GetInstance(colorGenerator_);
    CheckError_(rc, "create image generator instance");
    
    XnMapOutputMode frame_mode;

    // Set it to VGA maps at 30 FPS
    frame_mode.nXRes = XN_VGA_X_RES;
    frame_mode.nYRes = XN_VGA_Y_RES;
    frame_mode.nFPS = 30;

    rc = colorGenerator_.SetMapOutputMode(frame_mode);
    if (CheckError_(rc, "set color mode")) return 1;
    
    rc = colorGenerator_.SetIntProperty("InputFormat", 6);
    if (CheckError_(rc, "set bayer uncompressed")) return 1;
    rc = colorGenerator_.SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT);
    if (CheckError_(rc, "disable internal debayer")) return 1;
    
    colorMode_ = true;
    rc = colorGenerator_.StartGenerating();
    if (CheckError_(rc, "starting color cam")) return 1;
    pthread_mutex_unlock(exclusive_mutex);
    return 0;
  }

  int VideoSource_OpenNI::SetVideoModeIR() {
    pthread_mutex_lock(exclusive_mutex);
    if (colorMode_ == true) {
      if (colorGenerator_.IsValid() && colorGenerator_.IsGenerating()) {
        colorGenerator_.StopGenerating();
        colorGenerator_.Release();
      }
    } else {
      pthread_mutex_unlock(exclusive_mutex);
      return 0;
    }

    XnStatus rc;
    rc = context_.CreateProductionTree (devices_[deviceId_].irInfo);
    CheckError_(rc, "create ir generator");

    rc = devices_[deviceId_].irInfo.GetInstance(irGenerator_);
    CheckError_(rc, "create IR generator instance");
    
    XnMapOutputMode frame_mode;

    // Set it to VGA maps at 30 FPS
    frame_mode.nXRes = XN_VGA_X_RES;
    frame_mode.nYRes = XN_VGA_Y_RES;
    frame_mode.nFPS = 30;

    rc = irGenerator_.SetMapOutputMode(frame_mode);
    if (CheckError_(rc, "set ir mode")) return 1;
    
    colorMode_ = false;

    rc = irGenerator_.StartGenerating();
    if (CheckError_(rc, "starting ir cam")) return 1;
    pthread_mutex_unlock(exclusive_mutex);
    return 0;
  }
  
  int VideoSource_OpenNI::SetDepthCaptureMode(CaptureMode mode) {
    capMode_ = mode;
    return 0;
  }

  int VideoSource_OpenNI::SetSwitchVideoModeAfterGrab() {
    switchModeEveryFrame_ = !switchModeEveryFrame_;
    return 1;
  }
  
  int VideoSource_OpenNI::SetTilt(double degrees) {
    BIASERR("not yet implemented");
    return 1;
  }

  int VideoSource_OpenNI::SetLed(int mode) {
    BIASERR("not yet implemented");
    return 1;
  }
  
  int VideoSource_OpenNI::GetAccelerometer(double &tilt, double &x, double &y, double &z) {
    BIASERR("not yet implemented");
    return 1;
  }


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

  int VideoSource_OpenNI::GetCapabilities(const char *device, VideoSourceCapabilities &caps)
  {
    return 0;
  }
  
  void VideoSource_OpenNI::CreateLookupTable_(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));
      }
    }
  }
  
  void VideoSource_OpenNI::DeleteLookupTable_() {
    if (depthLookupTable_ != NULL) {
      delete depthLookupTable_;
      depthLookupTable_ = NULL;
    }
  }

  void VideoSource_OpenNI::ScanBus() {
    XnStatus rc;
    rc = context_.Init();
    CheckError_(rc, "init context");

    // enumerate all devices
    rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEVICE, NULL, device_nodes_);
    CheckError_(rc, "enumerate devices");

    vector<xn::NodeInfo> device_info;
    for (xn::NodeInfoList::Iterator nodeIt = device_nodes_.Begin (); nodeIt != device_nodes_.End (); ++nodeIt)
    {
      device_info.push_back (*nodeIt);
    }

    // enumerate depth nodes
    rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEPTH, NULL, depth_nodes_, NULL);
    CheckError_(rc, "enumerate depth generators");

    vector<xn::NodeInfo> depth_info;
    for (xn::NodeInfoList::Iterator nodeIt = depth_nodes_.Begin (); nodeIt != depth_nodes_.End (); ++nodeIt)
    {
      depth_info.push_back (*nodeIt);
    }

    // enumerate image nodes
    rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_IMAGE, NULL, image_nodes_, NULL);
    CheckError_(rc, "enumerate image generators");

    vector<xn::NodeInfo> image_info;
    for (xn::NodeInfoList::Iterator nodeIt = image_nodes_.Begin (); nodeIt != image_nodes_.End (); ++nodeIt)
    {
      image_info.push_back (*nodeIt);
    }

    // enumerate ir nodes
    rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_IR, NULL, ir_nodes_, NULL);
    CheckError_(rc, "enumerate ir generators");

    vector<xn::NodeInfo> ir_info;
    for (xn::NodeInfoList::Iterator nodeIt = ir_nodes_.Begin (); nodeIt != ir_nodes_.End (); ++nodeIt)
    {
      ir_info.push_back (*nodeIt);
    }

    for (unsigned int i=0;i<device_info.size();i++) {
      devices_.push_back( ONIDeviceNode( depth_info[i], image_info[i], ir_info[i], depth_info[i]));
    }

    numDevices_ = devices_.size();
    cout << "OpenNI: There are " << numDevices_ << " devices connected. " << endl;
    
  }

}