VideoSource_DcamPMD.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
#include <unistd.h>
#else
#include <Base/Common/W32Compat.hh>
#endif //WIN32

#include <string>
#include <sstream>
#include <iostream>

#include "VideoSource_DcamPMD.hh"
#include <Base/Image/ImageIO.hh>
#include <Image/Camera.hh>

#include <Base/Common/BIASpragma.hh>
#include <Base/Common/FileHandling.hh>

using namespace BIAS;
using namespace std;


BIAS::VideoSource_DcamPMD::
VideoSource_DcamPMD() : VideoSource()
{
  VideoSource::SetSize(64,48,1);
  VideoSource::SetColorModel(BIAS::ImageBase::CM_Grey);
  Width_ = 752;
  Height_ = 480;

}

BIAS::VideoSource_DcamPMD::
~VideoSource_DcamPMD()
{
}

int BIAS::VideoSource_DcamPMD::
OpenDevice()
{
  if (pmdConnectFireWire(&handle_) != PMD_OK) {
    BIASERR("Connection of PMD-Camera via FireWire failed!");
    return -1;
  }
  unsigned int width, height;
  if (pmdGetWidth(handle_, &width) != PMD_OK) {
    BIASERR("Error in pmdGetWidth!");
    return -1;
  }
  if (pmdGetHeight(handle_, &height) != PMD_OK) {
    BIASERR("Error in pmdGetWith!");
    return -1;
  }
  if (pmdGetPixelOrigin(handle_, &pixori_) != PMD_OK) {
   BIASERR("Error in pmdGetPixelOrigin!");
    return -1;
  }

  Width_ = width;
  Height_ = height;  
  DepthImg_.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  DepthImg_.SetColorModel(ImageBase::CM_Grey);
  ModCoeffImg_.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  ModCoeffImg_.SetColorModel(ImageBase::CM_Grey);

  return 0;
}

int BIAS::VideoSource_DcamPMD::
CloseDevice()
{
  if (pmdDisconnect(handle_) != PMD_OK) {
    BIASERR("Disonnection of PMD-Camera via FireWire failed!");
    return -1;
  }
  return 0;
}

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


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

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


int BIAS::VideoSource_DcamPMD::SetIntegrationTime(int dIntegrationTime){
  return pmdSetIntegrationTime(handle_, dIntegrationTime);
}

int BIAS::VideoSource_DcamPMD::GetIntegrationTime(){
  unsigned int time =0;
  int ret = pmdGetIntegrationTime(handle_,&time);
  if(ret >=0)
    return (int)time;
  else
    return ret;
}

int BIAS::VideoSource_DcamPMD::SetModulationFrequency(int dModulationFrequency){
  return pmdSetModulationFrequency(handle_, dModulationFrequency);
}


int BIAS::VideoSource_DcamPMD::GetModulationFrequency(){
  unsigned int frequency =0;
  int ret = pmdGetModulationFrequency(handle_,&frequency);
  if(ret >=0)
    return (int)frequency;
  else
    return ret;
}



int VideoSource_DcamPMD::SetGain(float gain)
{
  BIASERR("Setting gain not supported for PMD cam");
  return 0;
}

int VideoSource_DcamPMD::SetShutter(float shutter)
{
  BIASERR("Setting shutter not supported for PMD cam");
  return 0;
}

float VideoSource_DcamPMD::GetShutter(){
  BIASERR("Getting shutter not supported for PMD cam");
  return 0.0;
}

float VideoSource_DcamPMD::GetGain(){
  BIASERR("Getting gain not supported for PMD cam");
  return 0.0;
}

int BIAS::VideoSource_DcamPMD::GrabSingle(Camera<unsigned char> &image)
{

  int res=0;
  if ((res=pmdUpdate(handle_)) != PMD_OK) {
    BIASERR("Error in pmdUpdate! Error: " << res);
    return -1;
  };

  double *data;
  if ((res=pmdGetIntensities(handle_, (void**)&data)) != PMD_OK) {
    BIASERR("Error in pmdGetIntensities! Error: " << res);
    return -1;
  };
  ImageUcharFromDouble_(data, image);
  
   timeval tv;
   gettimeofday(&tv, NULL);
   image.SetTime(tv.tv_sec,tv.tv_usec);
 
  if ((res=pmdGetDistances(handle_, (void**)&data)) != PMD_OK) {
    BIASERR("Error in pmdGetDistances! Error: " << res);
    return -1;
  };
  ImageFloatFromDouble_(data, DepthImg_);
  DepthImg_.SetUID(image.GetUID());  

  if ((res=pmdGetAmplitudes(handle_, (void**)&data)) != PMD_OK) {
    BIASERR("Error in pmdGetAmplitudes! Error: " << res);
    return -1;
  };
  ImageFloatFromDouble_(data, ModCoeffImg_);
  ModCoeffImg_.SetUID(image.GetUID());  
  return 0;
}


double BIAS::VideoSource_DcamPMD::GetFPS()
{
  double* fps=NULL;
  if (pmdGetInternalFrameRate(handle_, fps) != PMD_OK) {
    BIASERR("Error in pmdGetInternalFrameRate!");
    return -1;
  }
  return *fps;
}

 
void BIAS::VideoSource_DcamPMD::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++;
  }
  pus=data;
  unsigned char** puc=dst.GetImageDataArray();
  unsigned char*  pucrow;
  for (unsigned int x=0; x<dst.GetHeight(); x++) {
    switch (pixori_) {
      case 1: pucrow=puc[x]; break;
      case 2: pucrow=puc[x]+dst.GetWidth()-1; break;
      case 3: pucrow=puc[dst.GetHeight()-x]; break;
      case 4: pucrow=puc[dst.GetHeight()-x]+dst.GetWidth()-1; break;
      default: BIASERR("Unknown Pixel Origin"); BIASABORT;
    }
    for (unsigned int y=0; y<dst.GetWidth(); y++) {
      (*pucrow) = (unsigned char)((double)((*pus)-min)/(double)(max-min)*255.0);
      switch (pixori_) {
        case 1:
        case 3: pucrow++; break;
        case 2:
        case 4: pucrow--; break;
        default: BIASERR("Unknown Pixel Origin"); BIASABORT;
      }      
      pus++;
    }
  }
 }


void BIAS::VideoSource_DcamPMD::ImageFloatFromDouble_(double* data, Image<float> &dst)
{ 
  if (dst.IsEmpty()) {
    dst.Init(Width_, Height_, ColorChannels_, ImageBase::ST_float);
  }

  double* pus=data;
  float** puc=dst.GetImageDataArray();
  float*  pucrow;
  for (unsigned int x=0; x<dst.GetHeight(); x++) {
    switch (pixori_) {
      case 1: pucrow=puc[x]; break;
      case 2: pucrow=puc[x]+dst.GetWidth()-1; break;
      case 3: pucrow=puc[dst.GetHeight()-x]; break;
      case 4: pucrow=puc[dst.GetHeight()-x]+dst.GetWidth()-1; break;
      default: BIASERR("Unknown Pixel Origin"); BIASABORT;
    }
    for (unsigned int y=0; y<dst.GetWidth(); y++) {
      (*pucrow) = (float)(*pus);
      switch (pixori_) {
        case 1:
        case 3: pucrow++; break;
        case 2:
        case 4: pucrow--; break;
        default: BIASERR("Unknown Pixel Origin"); BIASABORT;
      }      
      pus++;
    }
  }
}