VideoSource_SwissRanger.cpp

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

Copyright (C) 2003-2009    (see file CONTACT 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
*/

#ifdef _WIN32
#pragma comment( lib, "libMesaSR.lib" )
#endif


//THE header
#include "VideoSource_SwissRanger.hh"

//std stuff
#include <string>
#include <sstream>
#include <iostream>

//BIAS stuff
#include <Image/Camera.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Image/ImageIO.hh>


//mesa swissranger stuff
#include <libMesaSR.h>

#ifndef WIN32
#include <usb.h>
#endif

using namespace BIAS;
using namespace std;


BIAS::VideoSource_SwissRanger::
VideoSource_SwissRanger() : VideoSource()
{
  //for VideoSource-class
  VideoSource::SetSize(-1,-1,1);
  VideoSource::SetColorModel(BIAS::ImageBase::CM_Grey);
  Width_ = -1;
  Height_ = -1;
  srCam_ = NULL;
  bIsOpen_ = false;
  serialNumber_=0;
  amplitudeThreshold_ = 0;
  saturationThreshold_ = 0;
  integrationTime_ = 80;
  distanceOffset_ = 0;
  modulationFrequency_ = MF_20MHz;
  bEnsureLatestFrame_=false;
  dataDepth_ = NULL;
  dataAmp_ = NULL;
  bGrabImageReturnsDepth_ = false;
  camType_ = CT_UNKNOWN;
  bRetryOpen_ = false;
}

BIAS::VideoSource_SwissRanger::
~VideoSource_SwissRanger()
{
  if (bIsOpen_) CloseDevice();
}

int VideoSource_SwissRanger::
GetSoftwareVersion(std::string& version){
    unsigned short v[4];
    int ret = SR_GetVersion(v);
    stringstream ver;
    ver<<v[3]<<"."<<v[2]<<"."<<v[1]<<"."<<v[0];
    version = ver.str();
    return ret;
}

int VideoSource_SwissRanger::
GetDeviceString(std::string &device){
    if(srCam_== NULL) return -1;
    char buf[100];
    int ret = SR_GetDeviceString(srCam_,buf,100);
    device = string(buf);
    return ret;
}

int BIAS::VideoSource_SwissRanger::
OpenDevice()
{
    int ret = 0;
  if (bIsOpen_) {
    BIASERR("Device SwissRanger has been opened already.");
    return -1;
  }
  string version;
  ret = GetSoftwareVersion(version);
#ifdef BIAS_DEBUG
  cout<<"SRMesa Software Version:"<<version<<endl;
#endif

  //SR_SetTimeout(srCam_,20000);
  //ret = SR_OpenDlg(&srCam_,2,0);
  ret = SR_OpenUSB(&srCam_,serialNumber_);

  if ( ret <= 0) {
    if (bRetryOpen_) {
        BIASERR("OpenDevice of Swissranger not successfull (return:"<< ret<<") try to reopen.")
      bRetryOpen_ = false;
      CloseDevice();
      ResetUsbDeviceConnection_();
      return OpenDevice();
    } else {
      BIASERR("Could not open the SwissRanger device (return:"<< ret<<")!");
      return -1;
    }
  }

  Width_ = SR_GetCols(srCam_);
  Height_ = SR_GetRows(srCam_);
  
  if (Width_ < 10) {
    if (bRetryOpen_) {
        BIASERR("Image dimension of Swissranger not correct, try to reopen.")
      bRetryOpen_ = false;
      CloseDevice();
      ResetUsbDeviceConnection_();
      return OpenDevice();
    } else {
      BIASERR("Connection to Swissranger is not sane. Please restart.");
      return -2;
    }
  }

  ColorChannels_ = 1;

  int  numPics = 0;
  numPics = SR_GetImageList(srCam_,&imgEntryArray);

  ImgEntry entr;
  for(int i = 0; i < numPics; i++){
    entr = imgEntryArray[i];
    #ifdef BIAS_DEBUG
    cout << "ImageType: " << entr.imgType << endl;
    #endif
  }

  
  DepthImg_.Init(Width_, Height_, 1, ImageBase::ST_unsignedchar);
  DepthImg_.SetColorModel(ImageBase::CM_Grey);
  DepthImgF_.Init(Width_, Height_, 1, ImageBase::ST_float);
  DepthImgF_.SetColorModel(ImageBase::CM_Grey);

  ModCoeffImg_.Init(Width_, Height_, 1, ImageBase::ST_float);
  ModCoeffImg_.SetColorModel(ImageBase::CM_Grey);

  SR_SetIntegrationTime(srCam_,integrationTime_);
  //if(modulationFrequency_ != MF_20MHz)
  ret = SR_SetModulationFrequency(srCam_,modulationFrequency_);
  if(ret != 0) BIASERR("Setting modulation Frequency failed.");
  modulationFrequency_ = SR_GetModulationFrequency(srCam_); 
  SetMaxDist_();

  // api provides access to get device string, but lacks obviously needed get cam type
  string devstring; GetDeviceString(devstring);
  if (devstring.find("SR3000") != string::npos) {
    camType_ = CT_SR3K_USB;
  } else {
    camType_ = CT_SR4K_USB;
  }

  if(bExternalTrigger_){
    #ifdef BIAS_DEBUG
      cout<<"SR Setting mode with Trigger!"<<endl;
    #endif
    ret = SR_SetMode(srCam_,AM_COR_FIX_PTRN|AM_MEDIAN|AM_DENOISE_ANF|AM_CONV_GRAY|AM_HW_TRIGGER|AM_SW_TRIGGER);
  }
  else{
    //if AM_SW_TRIGGER (SOFTWARE TRIGGER) is set, fps are reduced, because taking image starts by call to SR_ACQUIRE  
    //still better than ensure latest!
    #ifdef BIAS_DEBUG
      cout<<"SR Setting normal mode!"<<endl;
    #endif
    ret = SR_SetMode(srCam_,AM_COR_FIX_PTRN|AM_MEDIAN|AM_CONV_GRAY|AM_DENOISE_ANF);
  }

  if (ret < 0) {
    BIASERR("Opening Swissranger failed!");
    return ret;
  }
    
  bIsOpen_ = true;
  return 0;
}

int BIAS::VideoSource_SwissRanger::
OpenDevice(int camNr)
{
  serialNumber_= camNr;
  return OpenDevice();
}

int BIAS::VideoSource_SwissRanger::
CloseDevice()
{
  if (SR_Close(srCam_) < 0) {
    BIASERR("Disonnection of Swiss Ranger via USB failed!");
    return -1;
  }

  bIsOpen_ = false;
  //cout<<"Closed Swissranger Camera"<<endl;
  return 0;
}

int BIAS::VideoSource_SwissRanger::
SetIntegrationTimeMS(float inttime) {
  float val = 0.0f; 
  switch (camType_) {
    case CT_SR3K_ETH:
    case CT_SR3K_USB:
      val = 0.25f * ((inttime / 0.2f) - 1.0f);
      break;    
    case CT_SR4K_ETH:
    case CT_SR4K_USB:
      val = 0.25f * ((inttime - 0.3f) / 0.1f);
      break;
    default:
      BIASERR("can not set integration time as camera type is unknown.");
      return -1;
  }
  if (val < 0 || val > 255) {
    BIASWARN("clipping integration time");
  }
  return SetIntegrationTime( (unsigned char)val );
}

int BIAS::VideoSource_SwissRanger::
GetIntegrationTimeMS(float &inttime) {
  switch (camType_) {
    case CT_SR3K_ETH:
    case CT_SR3K_USB:
      inttime = 4.0f * float(integrationTime_+1.0f)*0.2f;
      break;    
    case CT_SR4K_ETH:
    case CT_SR4K_USB:
      inttime = 4.0f * (0.3f+float(integrationTime_)*0.1f);
      break;
    default:
      BIASERR("can not set integration time as camera type is unknown.");
      return -1;
  }
  return 0;
}


int BIAS::VideoSource_SwissRanger::
PreGrab()
{
  return 0;
}


int BIAS::VideoSource_SwissRanger::
PostGrab()
{
  return 0;
}

int BIAS::VideoSource_SwissRanger::
InitImage(ImageBase &img) {
  if (!img.IsEmpty()){
    BIASERR("VideoSource::InitImage() should be called with uninitialized image");
    img.Release();
  }
  img.Init(Width_, Height_, ColorChannels_, ImageBase::ST_unsignedchar);
  img.SetColorModel(ImageBase::CM_Grey);
  return 0;
}

int BIAS::VideoSource_SwissRanger::
InitDepthImage(ImageBase &img) {
  if (!img.IsEmpty()){
    BIASERR("VideoSource::InitImage() should be called with uninitialized image");
    img.Release();
  }
  img.Init(Width_, Height_, 1, ImageBase::ST_float);
  img.SetColorModel(ImageBase::CM_Grey);
  return 0;
}


int BIAS::VideoSource_SwissRanger::
GrabSingle(Camera<unsigned char> &image)
{
  int res=0;

  if (!bIsOpen_) {
    BIASERR("GrabSingle called but device not opened!");
    return -1;
  }

  res=SR_Acquire(srCam_);

  if(bEnsureLatestFrame_)
    res+=SR_Acquire(srCam_);
 
  if (res<=0) {
    if(bExternalTrigger_){
        BIASERR("Reading image from SwissRanger failed. Wait for next trigger.");
        return res;
    }else{
        BIASERR("Reading image from SwissRanger failed. Please restart application");
        exit(-1);    
    }
  }

  SR_GetImageList(srCam_,&imgEntryArray);

  dataDepth_ = (const unsigned short*)SR_GetImage(srCam_,0);
  dataAmp_ = (const unsigned short*)SR_GetImage(srCam_,1);

  ConvertRawImageToFloat_(dataDepth_, DepthImgF_);
  ConvertRawImage_(dataAmp_, ModCoeffImg_);
  if (bGrabImageReturnsDepth_) {
    ConvertImageFloatToCharNormalized_(DepthImgF_, image, true);
  } else {
    ConvertImageFloatToCharNormalized_(ModCoeffImg_, image);
  }

  return res;
}

void BIAS::VideoSource_SwissRanger::
ConvertImageFloatToCharNormalized_(const Image<float> src,
                                   Image<unsigned char> &dst, bool useMaxDist)
{
  dst.ReInit(Width_, Height_, 1, ImageBase::ST_unsignedchar);
  float normFactor = 0.0f;
  if (useMaxDist) {
    normFactor = float(255.0f / max_dist_);
  } else {
    float min,max;
    src.GetMinMaxPixelValue(min,max);
    normFactor = float(255.0f / max);
  }
  const float** srcData = src.GetImageDataArray();
  unsigned char** dstData=dst.GetImageDataArray();
  for (unsigned int y=0; y<dst.GetHeight(); y++) {
    for (unsigned int x=0; x< dst.GetWidth(); x++) {
      dstData[y][x] = (unsigned char)( srcData[y][x] * normFactor );
    }
  }
}

void BIAS::VideoSource_SwissRanger::
ConvertRawImageToFloat_(const unsigned short* data,
                        Image<float> &dst)
{
  if (dst.IsEmpty()) {
    dst.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  }

  const unsigned short* srcDepthP=data;
  float** dstData=dst.GetImageDataArray();
  for (unsigned int y=0; y<dst.GetHeight(); y++) {
    for (unsigned int x=0; x< dst.GetWidth(); x++) {
      float cur = (*srcDepthP)*max_dist_ / (float)0xffff;
      dstData[y][x] = cur;
      srcDepthP++;
    }
  }
}

void BIAS::VideoSource_SwissRanger::
ConvertRawImage_(const unsigned short* data,
                 Image<float> &dst)
{
  if (dst.IsEmpty()) {
    dst.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  }

  const unsigned short* srcDepthP=data;
  float** dstData=dst.GetImageDataArray();
  float step = (float)0xff / (float)0xffff;
  for (unsigned int y=0; y<dst.GetHeight(); y++) {
    for (unsigned int x=0; x< dst.GetWidth(); x++) {
      float cur = (*srcDepthP) * step;
      dstData[y][x] = cur;
      srcDepthP++;
    }
  }
}

int BIAS::VideoSource_SwissRanger::SetIntegrationTime(const unsigned char inttime) {
  integrationTime_ = inttime;
  return SR_SetIntegrationTime(srCam_, inttime);
}

int BIAS::VideoSource_SwissRanger::SetModulationFrequency(ModulationFrq dModulationFrequency) {
  modulationFrequency_ = dModulationFrequency;
  SetMaxDist_();
  return SR_SetModulationFrequency(srCam_, (ModulationFrq)modulationFrequency_);
}

void BIAS::VideoSource_SwissRanger::SetMaxDist_() {
  float lightspeed = 299792.485f;
#ifdef BIAS_DEBUG
  std::cout<<"modulationFrequency_:"<< modulationFrequency_ <<std::endl;
#endif

  switch (modulationFrequency_) {
  case MF_60MHz:   //!<SR4k: maximal range 2.5m \intDoc{frequency}
    max_dist_ = (lightspeed / 60.0f) / 2.0f;
    break;
  case MF_40MHz: //!<SR3k: maximal range 3.75m
    max_dist_ = (lightspeed / 40.0f) / 2.0f;
    break;
  case MF_31MHz:
    max_dist_ = (lightspeed / 31.0f) / 2.0f;
    break;
  case MF_30MHz:
    max_dist_ = (lightspeed / 30.0f) / 2.0f;
    break;
  case MF_29MHz:
    max_dist_ = (lightspeed / 29.0f) / 2.0f;
    break;
  case MF_21MHz:
    max_dist_ = (lightspeed / 21.0f) / 2.0f;
    break;
  case MF_20MHz:
    max_dist_ = (lightspeed / 20.0f) / 2.0f;
    break;
  case MF_19MHz:
    max_dist_ = (lightspeed / 19.0f) / 2.0f;
    break;
  case MF_15MHz:
    max_dist_ = (lightspeed / 15.0f) / 2.0f;
    break;
  case MF_10MHz:
    max_dist_ = (lightspeed / 10.0f) / 2.0f;
    break;   
  case MF_LAST:
    break;
  default:
    BIASERR("Can not set modulation frequency for SwissRanger!");
    break;
  }
}

void BIAS::VideoSource_SwissRanger::ResetUsbDeviceConnection_() {
#ifndef WIN32
  // inform user
  BIASWARN("resetting usb device. device will be set to default configuration");
  // structs holding usb information
  struct usb_bus *busses;
  struct usb_bus *bus;
  struct usb_device *dev;
  struct usb_dev_handle *handle;
  // init usb and find all devices on all busses
  usb_init();
  usb_find_busses();
  usb_find_devices();

  // walk busses and devices until a device matching swissranger vendor and product id is found
  busses = usb_get_busses();
  for (bus = busses; bus; bus = bus->next) {
    for (dev = bus->devices; dev; dev = dev->next) {
      if ((dev->descriptor.idVendor == 0x0852 || dev->descriptor.idVendor == 0x1ad2) && (dev->descriptor.idProduct == 0x0074 || dev->descriptor.idProduct == 0x0075)) {
        // open device and set to default config
        handle = usb_open(dev);
        usb_reset(handle);
        usb_set_configuration(handle, dev->config[0].bConfigurationValue);
        usb_claim_interface(handle, 0);
        // close it again
        usb_close(handle);
      }
    }
  }
#endif
}

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

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