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

#ifndef  WIN32
#  error The PMD-cameras can only be run under windows.
#endif //not WIN32

//THE header
#include "VideoSource_PMDZess.hh"

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

//BIAS stuff
#include <Image/Camera.hh>

//PMD stuff
#include <PMDDll.h>
#include <defines.hpp>

using namespace BIAS;
using namespace std;


BIAS::VideoSource_PMDZess::VideoSource_PMDZess() 
  : VideoSource()
{
  //for VideoSource-class
  VideoSource::SetSize(-1,-1,1);
  VideoSource::SetColorModel(BIAS::ImageBase::CM_Grey);
  Width_ = -1;
  Height_ = -1;

  //own standards
  type_ = MultiCam_3K;
  use2D_ = 1;
  doCalcs_ = 1;
  phaseAlgorithm_ = 4;
  modulationsFrequenz_ = 20;
  bIsOpen_ = false;
  devicePort_ = "USB0";
  bitFile_ = "";
  dist_ = NULL;
  mod_ = NULL;
  grey_ = NULL;
  total_ = NULL;
  grey2D_ = NULL;
}

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

int BIAS::VideoSource_PMDZess::OpenDevice()
{
  if (bIsOpen_) {
    BIASERR("Device has been opened allready!!!");
    return -1;
  }

  if (bitFile_ == "") {
    BIASERR("No bitfile for PMDZess given!!!");
    return -1;
  }

  use2D_ = 0;
  doCalcs_ = 1;
  phaseAlgorithm_ = 4;

  //TODO: activate 2D-output -> set 2.param to 1 (docum. see PMDDll.h)
  if (z2D3D_setChip(type_, use2D_, doCalcs_, phaseAlgorithm_)<0) {
    BIASERR("Connection to PMD-Camera via USB failed!");
    return -1;
  }

  //first param ?will get? used, if more than one cam is attaced via USB (N/A now)!
  //Parameter NULL is passed for standard phase-shifts! (consult docs before using)
  int ret = z2D3D_init(devicePort_.c_str(), bitFile_.c_str(), type_, use2D_,
                 modulationsFrequenz_, 0, doCalcs_, phaseAlgorithm_);
  if ( ret < 0) {
    char message[200];
    int errorcode=0;
    ret = z2D3D_getlasterror(message, 200,&errorcode);
    cout<<"Error:"<<message<<endl;
    BIASERR("Error initializing camera!!!"<<ret);
    return -1;
  }

  
  switch(type_) {
  case PMD_1K :
    Width_ = 64;
    Height_ = 16;
    break;
  case PMD_3K :
    Width_ = 64;
    Height_ = 48;
    break;
  case MultiCam_3K :
    Width_ = 64;
    Height_ = 48;
     //addept to new vals when 2d-imaging is supported
    Width2D_ = 640;
    Height2D_ = 480;
    break;
  case MultiCam_19K :
    Width_ = 160;
    Height_ = 120;
     //addept to new vals when 2d-imaging is supported
    Width2D_ = 640;
    Height2D_ = 480;
    break;
  default:
    BIASERR("???Unknown PMD-type but init(...) succeeded???");
    return -1;
  }

  //set 3D-exposuretime to 2[ms] (= 2/(999 + 1) * 1000)
  z2D3D_setexpo( 2, 999,1);
  //set 2D-exposuretime to  5,1[ms] ( = 5ms * frametime(=1) + 10µs * rowtime(=10) )
  z2D3D_SetExposureTimeInMS_2D( 10, 1,999 );

  //set standard application mode
  if (z2D3D_SetApplicationMode(1) < 0) {
    BIASERR("Error setting application-mode 1!!!");
    //needed if OpenDevice was not completed
    if (z2D3D_deinit() < 0) 
      BIASERR("Disonnection of PMD-Camera via USB failed!");

    return -1;
  }

  //don't mirror/rotate anything CAUTION This is bitwise setting!
  unsigned long mirrorRotOpts = 0;
  if (z2D3D_MirrorRotOpts(&mirrorRotOpts , 1) < 0) {
    BIASERR("Error setting mirror-modes to none!!!");
    //needed if OpenDevice was not completed
    if (z2D3D_deinit() < 0) 
      BIASERR("Disonnection of PMD-Camera via USB failed!");

    return -1;
  }

  //activate metric depth-image
  if (z2D3D_Scale(1) < 0) {
    BIASERR("Error activating metric depth-output!!!");
    //needed if OpenDevice was not completed
    if (z2D3D_deinit() < 0) 
      BIASERR("Disonnection of PMD-Camera via USB failed!");

    return -1;
  }

  //prepare mem for one image (processing image by image)
  if (z2D3D_Prepare(1) < 0) {
    BIASERR("Error reserving memory for PMD-library!!!");
    //needed if OpenDevice was not completed
    if (z2D3D_deinit() < 0) 
      BIASERR("Disonnection of PMD-Camera via USB failed!");

    return -1;
  }

  dist_ = new double[Width_*Height_*ColorChannels_]; //depth image
  mod_ = new double[Width_*Height_*ColorChannels_]; //Coefficient image
  grey_ = 
    new signed short[Width_*Height_*ColorChannels_]; //PMD-grey image
  total_ = 
    new signed short[((Width_*Height_+4)*2)*phaseAlgorithm_]; //not needed (raw img data)
  grey2D_ = 
    new unsigned char[Width2D_*Height2D_*ColorChannels_]; //high res grey image

  if ((dist_ == NULL)||(mod_ == NULL)||(grey_ == NULL)||
      (total_ == NULL)||(grey2D_ == NULL))
  {
    BIASERR("Error allocating memory for temp-images!!!");
    //needed if OpenDevice was not completed
    if (z2D3D_deinit() < 0) 
      BIASERR("Disonnection of PMD-Camera via USB failed!");

    return -1;
  }

  //MIP 2D-grey image (not filled right now, templated <unsigned char>)
  img2DPMD_.Init(Width_, Height_, ColorChannels_, ImageBase::ST_unsignedchar);
  img2DPMD_.SetColorModel(ImageBase::CM_Grey);
  //MIP depth image (templated <float>)
  DepthImg_.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  DepthImg_.SetColorModel(ImageBase::CM_Grey);
  //MIP image holding Modulation-Coeefincients (templated <float>)
  ModCoeffImg_.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  ModCoeffImg_.SetColorModel(ImageBase::CM_Grey);
  imgPMD2D_.Init(Width2D_, Height2D_, ColorChannels_, ImageBase::ST_unsignedchar);
  imgPMD2D_.SetColorModel(ImageBase::CM_Grey);
  bIsOpen_ = true;

  return 0;
}

int BIAS::VideoSource_PMDZess::OpenDevice(int camNr)
{
  //set string for desired port
  std::stringstream str("USB");
  str << camNr;
  devicePort_ = str.str();
  //call standard open
  return OpenDevice();
}

int BIAS::VideoSource_PMDZess::CloseDevice()
{
  if (!bIsOpen_) {
    BIASERR("Cannot close device, it has not been opened successfully!!!");
    return -1;
  }

  if (z2D3D_deinit() < 0) {
    BIASERR("Disonnection of PMD-Camera via USB failed!");
    return -1;
  }

  delete dist_; //depth image
  delete mod_; //Coefficient image
  delete grey_; //PMD-grey image
  delete total_; //not needed (raw img data)
  delete grey2D_; //high resolution 2D image

  bIsOpen_ = false;

  return 0;
}

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


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

int BIAS::VideoSource_PMDZess::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_PMDZess::Init2DImage(ImageBase &img) {
    if (!img.IsEmpty()){
    BIASERR("VideoSource::InitImage() should be called with uninitialized image");
      img.Release();
    }
  img.Init(Width2D_, Height2D_, ColorChannels_, ImageBase::ST_unsignedchar);
  img.SetColorModel(ImageBase::CM_Grey);
  return 0;
}


int BIAS::VideoSource_PMDZess::SetModulationFrequency(int dModulationFrequency){
  unsigned long f = (unsigned long)(dModulationFrequency);
  //return z2D3D_ModulationFrequency(&f, 1);
  modulationsFrequenz_ = f;
  return 0;
}


int BIAS::VideoSource_PMDZess::GetModulationFrequency(){
//  unsigned long f;
  //int res = z2D3D_ModulationFrequency(&f, 0);
  ////check fo errors
  //if (res <= 0) return res;
  ////everything's fine, return
  //return (int)(f);
  return modulationsFrequenz_;
}


int BIAS::VideoSource_PMDZess::SetShutter3D(float exptime){
  //exptime [ms] = value * (prescaler + 1) / 1000 --> some conversion needed
  unsigned long value, prescaler;
  prescaler = 999;
  //value = (unsigned long)(1000.0 * double(exptime));
  value =(unsigned long)exptime;

  return z2D3D_setexpo(value, prescaler, value);
}

float BIAS::VideoSource_PMDZess::GetShutter3D(){
  unsigned long *value = new unsigned long;
  unsigned long *value2 = new unsigned long;
  unsigned long *prescaler = new unsigned long;
  //get values from lib
  if (z2D3D_getexpo(value, prescaler, value2) < 0)
    BIASERR("Error getting shutter from cam!");

  //convert integer values to exposuretime in sec.
  float res = float(*value) * (float(*prescaler) + 1.0f) / 1000.0f;

  delete value;
  delete value2;
  delete prescaler;
  return res;
}

int BIAS::VideoSource_PMDZess::SetShutter2D(float exptime){
  //exptime [ms] = 5ms * frametime + 10µs * rowtime --> some conversion needed
  unsigned long rowtime, frametime;
  unsigned long tmp = (unsigned long)(exptime * 1000.0);
  tmp %= 5000;
  rowtime = (unsigned long)(rint(tmp/10.0));
  frametime = (unsigned long)(exptime) / 5;
  //correct for rowtime ==0 and ==1
  if(rowtime <=1)
  {
    rowtime = 2;
  }
  //frametime = (unsigned long)exptime;
  //rowtime = 10;
  cout<<"Setting rowtime:"<<rowtime<<" frametime:"<<frametime<<endl;
  int ret = z2D3D_SetExposureTimeInMS_2D(rowtime, frametime, 999);
  if(ret != 0){
    char message[200];
    int errorcode=0;
    ret = z2D3D_getlasterror(message, 200, &errorcode);
    cout<<"Error:"<<message<<endl;
  }
  return ret;
}

float BIAS::VideoSource_PMDZess::GetShutter2D(){
  //exptime [ms] = 5ms * frametime + 10µs * rowtime --> some conversion needed
  unsigned long *rowtime = new unsigned long;
  unsigned long *frametime = new unsigned long;
  unsigned long *prescaler = new unsigned long;
  if (z2D3D_GetExposureTimeInMS_2D(rowtime, frametime, prescaler) < 0){
    BIASERR("Error getting shutter from 2D-cam!");
    char message[200];
    int errorcode=0;
    z2D3D_getlasterror(message, 200,&errorcode);
    cout<<"Error:"<<message<<endl;
    return -1;
  }
  cout<<"getting rowtime:"<<*rowtime<<" frametime:"<<*frametime<<endl;  
  //convert integer values to exposuretime in sec.
  float res = float(*frametime) * 5.0f + 0.01f * float(*rowtime);

  delete rowtime;
  delete frametime;
  delete prescaler;
  return res;
}

int VideoSource_PMDZess::SetGain2D(float gain)
{
  unsigned long gainUL = (unsigned long)gain;
  int ret = z2D3D_SetGain2D(gainUL,gainUL,gainUL,gainUL,gainUL);
  return ret;
}

float VideoSource_PMDZess::GetGain2D(){
    unsigned long *   gain = new unsigned long;
    unsigned long *     gainblue= new unsigned long;
    unsigned long *     gainred= new unsigned long;
    unsigned long *     gaingreen1= new unsigned long;
    unsigned long *     gaingreen2= new unsigned long;
  int ret = z2D3D_GetGain2D(gain,gainblue,gainred,gaingreen1,gaingreen2);

  delete gain;
  delete gainblue;
  delete gainred;
  delete gaingreen1; 
  delete gaingreen2;
  float result = (float)(*gain);
  return result;
}

int BIAS::VideoSource_PMDZess::CaptureSinglePhase(Camera<unsigned char> &image){
   //(ImageNr from the container, 0 for phase image 1 for 2D image)
  int res = z2D3D_CalcSinglePhase(0);

  res = z2D3D_Do(-1);   
 /* nr  index of the image in memory which shall be transfered
    Phasenr     the phase for which you'd like to get the information for (-1..-4 or 7,1,5,3 etc.)
    raw     pointer to a field reserved for the raw data of the phase, may be zero, then data is not copied
    expA    may be zero, else expA is copied here
    expB    may be zero, else expB is copied here
    pre     may be zero, else pre is copied here
    framecounter    may be zero, else framecounter is copied here
    realtimeclock   may be zero, else realtimeclock is copied here*/

  signed short *raw;// *expA, *expB, *pre, *phase, *framecounter;
  raw = new signed short[Width_*Height_*ColorChannels_];
  //expA = new short[Width_*Height_*ColorChannels_];
  //expB = new short[Width_*Height_*ColorChannels_];
  //pre = new short[Width_*Height_*ColorChannels_];
  //phase = new short[Width_*Height_*ColorChannels_];
  //framecounter = new short;
  //int *realtimeclock = new int;
  res = z2D3D_Fetch1(0,-1,raw,0,0,0,0,0,0);
  /*delete realtimeclock;
  */
  return res;
}

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

  //clac next image (-1 for all stored imgs)
  if (z2D3D_Do(-1) < 0) {
    BIASERR("Error calculating next image!!!");
    return -1;
  }

  //get next image (index 0 in memory)
  res = z2D3D_Fetch(0, dist_, mod_, grey_, total_, grey2D_);

  if (res < 0) {
    BIASERR("Error getting image data from PMD-lib!!!");
    return res;
  }

  //convert from double/short to float/unsigned char
  DepthImageFloatFromDouble_(dist_, DepthImg_);
  ImageFloatFromDouble_(mod_, ModCoeffImg_);
  ImageUcharFromShort_(grey_, image);
  Convert2DImage_(grey2D_,img2D_);
  return 0;
}

void BIAS::VideoSource_PMDZess::ImageUcharFromDouble_(double* data,
                                                     Image<unsigned char> &dst)
{ 
  if (dst.IsEmpty()) {
    dst.Init(Width_, Height_, ColorChannels_, ImageBase::ST_unsignedchar);
  }
  
  double* pus=data;
  double min = DBL_MAX;
  double max = DBL_MIN;
  for (unsigned int i=0; i<dst.GetWidth()*dst.GetHeight(); i++) {
    if (*pus<min) min = *pus;
    if (*pus>max) max = *pus;
    pus++;
  }
  double divisor = max - min;

  pus=data;
  if (divisor != 0) {
    unsigned char** puc=dst.GetImageDataArray();
    for (unsigned int y=0; y<dst.GetHeight(); y++) {
      for (int x=0; x<int(dst.GetWidth()); x++) {
        puc[y][int(dst.GetWidth())-1-x] =
          (unsigned char)(255.0*((*pus)-min)/divisor);
        pus++;
      }
    }
  } else {
    dst.SetZero();
  }
}

void BIAS::VideoSource_PMDZess::ImageUcharFromShort_(signed short* data,
                                                    Image<unsigned char> &dst)
{ 
  if (dst.IsEmpty()) {
    dst.Init(Width_, Height_, ColorChannels_, ImageBase::ST_unsignedchar);
  }
  
  signed short* pus=data;
  signed short min = *pus;
  signed short max = *pus;
  for (unsigned int i=0; i<dst.GetWidth()*dst.GetHeight(); i++) {
    if (*pus<min) min = *pus;
    if (*pus>max) max = *pus;
    pus++;
  }
  double divisor = double(max - min);

  pus=data;
  if (divisor != 0) {
    unsigned char** puc=dst.GetImageDataArray();
    for (unsigned int y=0; y<dst.GetHeight(); y++) {
      for (int x=0; x<int(dst.GetWidth()); x++) {
        puc[y][int(dst.GetWidth())-1-x] = 
          (unsigned char)(255.0*(double(*pus)-min)/divisor);
        pus++;
      }
    }
  } else {
    dst.SetZero();
  }
}

void BIAS::VideoSource_PMDZess::DepthImageFloatFromDouble_(double* data,
                                                     Image<float> &dst)
{ 
  if (dst.IsEmpty()) {
    dst.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  }
  double f = 20000000.0;
  double c = 299792458.0*1000.0;
  float m = float(c/f/2);
  double* pus=data;
  float** puc=dst.GetImageDataArray();
  for (unsigned int y=0; y<dst.GetHeight(); y++) {
    for (int x=0; x<int(dst.GetWidth()); x++) {
      puc[y][int(dst.GetWidth())-1-x] = (float)(*pus);
      while(puc[y][int(dst.GetWidth())-1-x]<0.0){       
        puc[y][int(dst.GetWidth())-1-x]+=m;
      }
      while(puc[y][int(dst.GetWidth())-1-x]>m){       
        puc[y][int(dst.GetWidth())-1-x]-=m;
      }
      pus++;
    }
  }
}


void BIAS::VideoSource_PMDZess::ImageFloatFromDouble_(double* data,
                                                     Image<float> &dst)
{ 
  if (dst.IsEmpty()) {
    dst.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  }
  
  double* pus=data;
  float** puc=dst.GetImageDataArray();
  for (unsigned int y=0; y<dst.GetHeight(); y++) {
    for (int x=0; x<int(dst.GetWidth()); x++) {
      puc[y][int(dst.GetWidth())-1-x] = (float)(*pus);
      pus++;
    }
  }
}


void BIAS::VideoSource_PMDZess::Convert2DImage_(unsigned char* data,
                                               Image<unsigned char> &dst)
{ 
  if (dst.IsEmpty()) {
    dst.Init(Width2D_, Height2D_, ColorChannels_, ImageBase::ST_float);
  }
  
  unsigned char* srcDatap=data;
  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][int(dst.GetWidth())-1-x] = (*srcDatap);
      srcDatap++;
    }
  }
}