VideoSource_PMD.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,
 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
 */

// header
#include "VideoSource_PMD.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>

using namespace BIAS;
using namespace std;

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

    integrationTime_ = 70;
    distanceOffset_ = 0;
    modulationFrequency_ = 0;
    bEnsureLatestFrame_ = false;
    serialNumber_ = ""; // this means that first device will be opened
    sourcePluginIsOpen_ = false;
    procPluginIsOpen_ = false;
  expMode_ = NORMAL;
  procPluginPath_ = BIAS_PMD_PMDTec_PROCPLUGIN;
  sourcePluginPath_ = BIAS_PMD_PMDTec_SOURCEPLUGIN;
}

VideoSource_PMD::~VideoSource_PMD() {
    if (bIsOpen_ || procPluginIsOpen_ || sourcePluginIsOpen_)
        CloseDevice();
}

int VideoSource_PMD::OpenProcessingPlugin(std::string plugin) {
    int res;
  res = pmdOpenProcessingPlugin(&handle_, plugin.c_str(), "");
    if (res != PMD_OK) {
        BIASERR("could not open processing plugin" << endl);
        procPluginIsOpen_ = false;
    } else {
        procPluginIsOpen_ = true;
    }

    return res;
}

void VideoSource_PMD::
SetSourcePlugin(std::string plugin){
  sourcePluginPath_ = plugin;
}

void VideoSource_PMD::
SetProcessingPlugin(std::string plugin){
  procPluginPath_ = plugin;
}

int VideoSource_PMD::OpenSourcePlugin(std::string serial,std::string plugin) {
    serialNumber_ = serial;
    int res = pmdOpenSourcePlugin(&handle_, plugin.c_str(), serialNumber_.c_str());
    if (res != PMD_OK) {
        BIASERR("could not open source plugin" << endl);
        sourcePluginIsOpen_ = false;
        return -1;
    }
  else {
        res = pmdUpdate(handle_);
        if (res != PMD_OK) {
            BIASERR("Pmd Update failed from PMDTec Camera");
            return -1;
        }
        res = pmdGetSourceDataDescription(handle_, &datadesc_);
        if (res != PMD_OK) {
            BIASERR("Data description retrieval failed from PMDTec Camera");
            return -1;
        }
        sourcePluginIsOpen_ = true;
        res = 0;
    }

    return res;
}

int VideoSource_PMD::GrabSourceData(BIAS::Image<char>& sourceData) {
    if (bIsOpen_ || sourcePluginIsOpen_) {
        unsigned int sourceDataSize = datadesc_.size;
        if (sourceData.GetHeight() != 1 || sourceData.GetWidth()
                != sourceDataSize || sourceData.GetChannelCount() != 1) {
            sourceData.Release();
            sourceData.Init(sourceDataSize, 1, 1);
        }

        GrabSourceData(sourceData.GetImageData(), sourceDataSize);
        MetaData metaData;
        metaData.Add(AppData::MD_PMD_DataDesc, sizeof(datadesc_),
                (char*) &datadesc_);
        sourceData.SetMetaData(metaData);
    } else {
        BIASERR("call OpenDevice(...) or OpenSourcePlugin(...) first ");
        return -1;
    }

    return 0;
}

int VideoSource_PMD::CloseProcessingPlugin() {
    int ret = pmdClose(handle_);
    if (ret != PMD_OK) {
        BIASERR("closing processing plugin failed");
        return -1;
    }

    procPluginIsOpen_ = false;

    return ret;
}

bool VideoSource_PMD::IsSourcePluginOpen() {
    return sourcePluginIsOpen_;
}
bool VideoSource_PMD::IsProcessingPluginOpen() {
    return procPluginIsOpen_;
}

int VideoSource_PMD::CalcDistancesFromSourceData(BIAS::Image<char>& sourceData,
        BIAS::Image<float>& depthImage) {
    if (procPluginIsOpen_) {
        PMDDataDescription dd;
        MetaData* metaData = sourceData.GetMetaData();
        if (metaData != NULL) {
            AppData appData;
            if (metaData->Find(AppData::MD_PMD_DataDesc, appData) != -0) {
                BIASERR("no meta data for distance calculation found");
                return -1;
            }
            memcpy(&dd, appData.data, appData.length);
            CalcDistancesFromSourceData(sourceData.GetImageData(), dd,
                    depthImage);
        } else {
            BIASERR("no meta data for distance calculation found");
            return -1;
        }

    } else {
        BIASERR("call OpenProcessingPlugin first " << endl);
        return -1;
    }

    return 0;
}

int VideoSource_PMD::CalcIntensitiesFromSourceData(
        BIAS::Image<char>& sourceData, BIAS::Image<float>& intImage) {
    if (procPluginIsOpen_) {
        PMDDataDescription dd;
        MetaData* metaData = sourceData.GetMetaData();
        if (metaData != NULL) {
            AppData appData;
            if (metaData->Find(AppData::MD_PMD_DataDesc, appData) != 0) {
                BIASERR("no meta data for distance calculation found");
                return -1;
            }

            memcpy(&dd, appData.data, appData.length);
            CalcIntensitiesFromSourceData(sourceData.GetImageData(), dd,
                    intImage);
        } else {
            BIASERR("no meta data for distance calculation found");
            return -1;
        }

    } else {
        BIASERR("call OpenProcessingPlugin first " << endl);
        return -1;
    }

    return 0;
}
int VideoSource_PMD::CalcAmplitudesFromSourceData(
        BIAS::Image<char>& sourceData, BIAS::Image<float>& ampImage) {
    if (procPluginIsOpen_) {
        PMDDataDescription dd;
        MetaData* metaData = sourceData.GetMetaData();
        if (metaData != NULL) {
            AppData appData;
            if (metaData->Find(AppData::MD_PMD_DataDesc, appData) != 0) {
                BIASERR("no meta data for distance calculation found");
                return -1;
            }

            memcpy(&dd, appData.data, appData.length);
            CalcAmplitudesFromSourceData(sourceData.GetImageData(), dd,
                    ampImage);
        } else {
            BIASERR("no meta data for distance calculation found");
            return -1;
        }

    } else {
        BIASERR("call OpenProcessingPlugin first " << endl);
        return -1;
    }

    return 0;
}

int VideoSource_PMD::CalcDistancesFromSourceData(char* sourceData,
        PMDDataDescription dd, Image<float>& depthImage) {
    if (procPluginIsOpen_) {
        int res = 0;
        unsigned int width = dd.img.numColumns;
        unsigned int height = dd.img.numRows;

        if (depthImage.GetWidth() != width || depthImage.GetHeight() != height
                || depthImage.GetChannelCount() != 1) {
            depthImage.Release();
            depthImage.Init(width, height, 1);
        }

        res = pmdCalcDistances(handle_, depthImage.GetImageData(), width
                * height * sizeof(float), dd, sourceData);
        if (res != PMD_OK) {
            BIASERR("could not retrieve distances from the source data"<<endl);
        }
        return res;
    } else {
        BIASERR("call OpenProcessingPlugin first " << endl);
        return -1;
    }
}

int VideoSource_PMD::
CalcAmplitudesFromSourceData(char* sourceData,
        PMDDataDescription dd, Image<float>& ampImage)
{
    if (procPluginIsOpen_) {
        int res = 0;
        unsigned int width = dd.img.numColumns;
        unsigned int height = dd.img.numRows;

        if (ampImage.GetWidth() != width || ampImage.GetHeight() != height
                || ampImage.GetChannelCount() != 1) {
            ampImage.Release();
            ampImage.Init(width, height, 1);
        }

        res = pmdCalcAmplitudes(handle_, ampImage.GetImageData(), width
                * height * sizeof(float), dd, sourceData);
        if (res != PMD_OK) {
            BIASERR("could not retrieve amplitudes from the source data"<<endl);
        }
        return res;
    } else {
        BIASERR("call OpenProcessingPlugin first " << endl);
        return -1;
    }
}

int VideoSource_PMD::CalcIntensitiesFromSourceData(char* sourceData,
        PMDDataDescription dd, Image<float>& intImage) {
    if (procPluginIsOpen_) {
        int res = 0;
        unsigned int width = dd.img.numColumns;
        unsigned int height = dd.img.numRows;

        if (intImage.GetWidth() != width || intImage.GetHeight() != height
                || intImage.GetChannelCount() != 1) {
            intImage.Release();
            intImage.Init(width, height, 1);
        }

        res = pmdCalcIntensities(handle_, intImage.GetImageData(), width
                * height * sizeof(float), dd, sourceData);
        if (res != PMD_OK) {
            BIASERR("could not retrieve intensities from the source data"<<endl);
        }
        return res;
    } else {
        BIASERR("call OpenProcessingPlugin first " << endl);
        return -1;
    }
}

int VideoSource_PMD::
OpenDevice()
{
    //cout << "Opening" << endl;
    if (bIsOpen_) {
        BIASERR("Device PMDTec Camera has been opened already.");
        return -1;
    }

    int ret = 0;

    // connect to camera
  ret = pmdOpen(&handle_, sourcePluginPath_.c_str(),
    serialNumber_.c_str(), procPluginPath_.c_str(), "");
    if (ret != PMD_OK) {
    char err[256];
    /* The handle parameter in pmdGetLastError is 0
       because no connection was established */
    pmdGetLastError(0, err, 256);
    BIASERR("Connection to PMDTec Camera failed, error:"<<err);
        return -2;
    }

    ret = pmdUpdate(handle_);
    if (ret != PMD_OK) {
        BIASERR("Data retrieval failed from PMDTec Camera");
        return -1;
    }
    ret = pmdGetSourceDataDescription(handle_, &datadesc_);

    //datadesc_.
    if (ret != PMD_OK) {
        BIASERR("Failed to receive device parameters from PMDTec Camera");
        pmdClose(handle_);
        return -1;
    }

    cout << "Sensor Width: " << datadesc_.img.numColumns << endl;
    cout << "Sensor Height: " << datadesc_.img.numRows << endl;

    bIsOpen_ = true;

    Width_ = datadesc_.img.numColumns;
    Height_ = datadesc_.img.numRows;
    ret = GetIntegrationTime(integrationTime_);
    if (ret != PMD_OK) {
        BIASERR("Failed to receive integration time from PMDTec Camera " << ret);
        pmdClose(handle_);
        return -1;
    }
    ret = GetModulationFrequency(modulationFrequency_);
    if (ret != PMD_OK) {
        BIASERR("Failed to receive modulation freq from PMDTec Camera");
        pmdClose(handle_);
        return -1;
    }
  //set a integration time of 20 ms 
    SetIntegrationTimeMS(20);
  // 20 Mhz
  SetModulationFrequency(20000000);

  unsigned inttime;
    ret = GetIntegrationTimeMS(inttime);
    cout << "IntegrationTime [ms]: " << inttime << endl;
    cout << "ModulationFreq: " << modulationFrequency_ << endl;

    SetMaxDist_();

    cout << "MaxDist: " << max_dist_ << endl;

    InitAllImages_();

    return 0;
}

int BIAS::VideoSource_PMD::
OpenDevice(int camNr)
{
    BIASWARN("given cam number is ignored!");
    return OpenDevice();
}

int BIAS::VideoSource_PMD::
OpenDevice(std::string serialNumber)
{
    serialNumber_ = serialNumber;
    return OpenDevice();
}

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

    int ret = pmdClose(handle_);
    if (ret != PMD_OK) {
        BIASERR("Disconnection of CamCube via USB failed!");
        return -1;
    }

    bIsOpen_ = false;
    sourcePluginIsOpen_ = false;
    procPluginIsOpen_ = false;
    serialNumber_ = "";
    cout << "Closed PMDTec CamCube" << endl;
    return 0;
}

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

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

int BIAS::VideoSource_PMD::InitImage_(ImageBase &img,
        ImageBase::EStorageType st, ImageBase::EColorModel cm) {
    if (!img.IsEmpty()) {
        img.Release();
    }
    img.Init(Width_, Height_, 1, st);
    img.SetColorModel(cm);
    return 0;
}

int BIAS::VideoSource_PMD::GrabSourceData(char* sourceData, unsigned int size) {
    if (bIsOpen_ || sourcePluginIsOpen_) {
        //  cout << "starting pmd update " << endl;
        int res = pmdUpdate(handle_);
        //  cout << "pmd update res " << res << endl;

        if (res != PMD_OK) {
            BIASERR("Frame could not be acquired");
            return -1;
        }

        //cout << "starting pmd get source data " << endl;
        res = pmdGetSourceData(handle_, sourceData, size);
        //  cout << "pmd get source data res " << res << endl;

        if (res != PMD_OK) {
            BIASERR("Could not get source data");
            return -1;
        }
    } else {
        BIASERR("call OpenDevice(...) or OpenSourcePlugin(...) first ");
        return -1;
    }

    return 0;
}

int BIAS::VideoSource_PMD::GrabSingle(Camera<unsigned char> &image) {
    if (!bIsOpen_) {
        BIASERR("GrabSingle called but device not opened!");
        return -1;
    }

    if (pmdUpdate(handle_) != PMD_OK) {
        BIASERR("Frame could not be acquired");
        return -1;
    }

    if (pmdGetAmplitudes(handle_, idaTmp_, bufSz_) != PMD_OK) {
        BIASERR("GetAmplitudes failed");
        return -1;
    }
    FlopImage_(idaTmp_, idaMCIF_);

    if (pmdGetDistances(handle_, idaTmp_, bufSz_) != PMD_OK) {
        BIASERR("GetDistances failed");
        return -1;
    }
    FlopImage_(idaTmp_, idaDIF_);

    if (pmdGetIntensities(handle_, idaTmp_, bufSz_) != PMD_OK) {
        BIASERR("GetIntensities failed");
        return -1;
    }
    FlopImage_(idaTmp_, idaIIF_);

    unsigned int pixels = DepthImgF_.GetPixelCount();
    for (unsigned int i = 0; i < pixels; i++) {
        // meters to millimeters
        idaDIF_[i] *= 1000.0f;
        // millimeters to [0..255]
        idaDI_[i] = (unsigned char) (idaDIF_[i] * 255.0f / max_dist_);
    }
  if (bGrabImageReturnsDepth_) 
    tmpScale_ = DepthImgF_;
  else
    tmpScale_ = ModCoeffImgF_;
    tmpScale_.ScaleShiftBetween(0, 255);
    //cout << tmpScale_.GetStorageType() << " " << tmpScale_.GetColorModel() << endl;
    // cout << "-------------------------------" << endl;
    ImageConvert::ConvertST(tmpScale_, image, ImageBase::ST_unsignedchar);

    //  image = DepthImg_;
    return 0;
}

int BIAS::VideoSource_PMD::
SetIntegrationTime(const unsigned int inttime)
{
    unsigned int result;

  //here divide by 4 to set for a single channel
  unsigned time =   inttime / 4;

  //for(unsigned i=0;i<4;i++){
    if (pmdGetValidIntegrationTime(handle_, &result, 0, CloseTo, time)
        != PMD_OK) {
      BIASERR("Error getting a valid integration time");
      return -1;
    }
    if (time != result) {
      BIASWARN("Camera does not allow " << inttime << " as integration time. Setting to " << result*4 << " instead!");
    }
    if (pmdSetIntegrationTime(handle_, 0, result) != PMD_OK) {
      BIASERR("Error setting integration time");
    }
    //}
    integrationTime_ = result;
    return 0;
}

int BIAS::VideoSource_PMD::
SetModulationFrequency(const unsigned dModulationFrequency) {
    unsigned int result;
  //  for(unsigned i=0;i<4;i++){
    if (pmdGetValidModulationFrequency(handle_, &result, 0, CloseTo,
                                       dModulationFrequency) != PMD_OK) {
      BIASERR("Error getting a valid modulation frequency");
      return -1;
    }
    if (dModulationFrequency != result) {
      BIASWARN("Camera does not allow " << dModulationFrequency << " as modulation frequency. Setting to " << result << " instead!");
    }
    
    if (pmdSetModulationFrequency(handle_,0 , result) != PMD_OK) {
      BIASERR("Error setting modulation frequency");
    }
    // }
    modulationFrequency_ = result;
    SetMaxDist_();
    return 0;
}

int BIAS::VideoSource_PMD::SetDistanceOffset(const unsigned int in_millimeters) {
    float in_meters = float(in_millimeters) / 1000.0f;
    std::stringstream s;
    s.precision(4);
    s << "SetSoftOffset " << in_meters;
    if (pmdSourceCommand(handle_, 0, 0, s.str().c_str()) != PMD_OK) {
        BIASERR("setting distance offset failed");
        return -1;
    }
    distanceOffset_ = in_millimeters;
    return 0;
}

int BIAS::VideoSource_PMD::GetSerialNumber(std::string &serial) {
    char sn[16];
    if (pmdSourceCommand(handle_, sn, 16, "GetSerialNumber") != PMD_OK) {
        BIASERR("error retrieving serial number");
        return -1;
    }
    serial = sn;
    return 0;
}

int BIAS::VideoSource_PMD::SetROI(const unsigned int UpperLeftX,
        const unsigned int UpperLeftY, const unsigned int Width,
        const unsigned int Height) {
    std::stringstream s;
    s.precision(4);
    s << "SetROI " << UpperLeftX << " " << UpperLeftY << " " << Width << " "
            << Height;
    if (pmdSourceCommand(handle_, 0, 0, s.str().c_str()) != PMD_OK) {
        BIASERR("error setting ROI");
        return -1;
    }
    Width_ = Width;
    Height_ = Height;
    InitAllImages_();
    return 0;
}

int BIAS::VideoSource_PMD::SetFOV(const float fov) {
    std::stringstream s;
    s.precision(1);
    s << "SetFOV " << fov;
    if (pmdSourceCommand(handle_, 0, 0, s.str().c_str()) != PMD_OK) {
        BIASERR("error setting FOV");
        return -1;
    }
    return 0;
}

void BIAS::VideoSource_PMD::SetMaxDist_() {
    float lightspeed = 299792485.0f;
    max_dist_ = 1000.0f * (lightspeed / float(modulationFrequency_)) / 2.0f;
}

void BIAS::VideoSource_PMD::InitAllImages_() {
    InitImage_(DepthImg_, ImageBase::ST_unsignedchar, ImageBase::CM_Grey);
    InitImage_(DepthImgF_, ImageBase::ST_float, ImageBase::CM_Depth);
    InitImage_(ModCoeffImgF_, ImageBase::ST_float, ImageBase::CM_Grey);
    InitImage_(IntensityImg_, ImageBase::ST_unsignedchar, ImageBase::CM_Grey);
    InitImage_(IntensityImgF_, ImageBase::ST_float, ImageBase::CM_Grey);
    InitImage_(tmpImg_, ImageBase::ST_float, ImageBase::CM_Grey);

    bufSz_ = Width_ * Height_ * sizeof(float);

    idaDIF_ = DepthImgF_.GetImageData();
    idaMCIF_ = ModCoeffImgF_.GetImageData();
    idaIIF_ = IntensityImgF_.GetImageData();

    idaDI_ = DepthImg_.GetImageData();
    idaII_ = IntensityImg_.GetImageData();

    idaTmp_ = tmpImg_.GetImageData();

}

void BIAS::VideoSource_PMD::FlopImage_(float *src, float *dst) {
    for (int y = 0; y < Height_; y++) {
        for (int x = 0; x < Width_; x++) {
            dst[y * Width_ + x] = src[y * Width_ + Width_ - x - 1];
        }
    }
}


int VideoSource_PMD::SetIntegrationTimeMS(const unsigned int inttime){
    // normally it should be inttime(in milliseconds)* 1000
    // we give the intergration time for the whole image not for
  // a single shot of four
 return SetIntegrationTime(inttime*1000);
}

int VideoSource_PMD::
GetIntegrationTime(unsigned int &val)
{
  int ret = 0;
  if (bIsOpen_) {
    unsigned inttime;
    ret = pmdGetIntegrationTime(handle_, &inttime, 0);
    //std::cout<<"pmdGetInt:"<<inttime<<std::endl;
    val = inttime*4;
  }
  else{
    val = integrationTime_*4;
    ret = 1;
  }
  //std::cout<<"GetIntegrationTime returns:"<<val<<std::endl;
  return ret;
}

int VideoSource_PMD::
GetIntegrationTimeMS(unsigned int &val)
{
  GetIntegrationTime(val);
  val = (unsigned)rint(val/1000.0f);
  //std::cout<<"GetIntegrationTimeMS returns:"<<val<<std::endl;
  return 0;
}

PMDDataDescription& VideoSource_PMD::
GetDataDescription()
{
    return datadesc_;
}

int VideoSource_PMD::
SetExposureMode(EXPOSURE_MODE mode)
{
  if (bIsOpen_ || sourcePluginIsOpen_)
    {
      int res;
      if (mode == SMB)
        {
          res = pmdSourceCommand(handle_, 0, 0, "SetExposureMode SMB");
          if (res == PMD_OK) {
              expMode_ = mode;
          } else {
              BIASERR("could not set exposure mode to smb");
              return -1;
          }
        }
      else
        {
          res = pmdSourceCommand(handle_, 0, 0, "SetExposureMode Normal");
          if (res == PMD_OK) {
              expMode_ = mode;
          } else {
              BIASERR("could not set exposure mode to Normal");
              return -2;
          }
        }
      return 0;
    }
  else
    {
      BIASERR("Device note opened ! Call OpenDevice(...) first !");
      return -1;
    }
}

EXPOSURE_MODE VideoSource_PMD::
GetExposureMode()
{
  return expMode_;
}

/* Get the Modulation frequency*/
int VideoSource_PMD::
GetModulationFrequency(unsigned int &val)
{
  val = modulationFrequency_;
  return 0;
}

/* Get Distance Offset */
int VideoSource_PMD::
GetDistanceOffset(unsigned int &millimeters)
{
  millimeters = distanceOffset_;
  return 0;
}

/** Returns the depth image. Must be called after GrabSingle() */
int VideoSource_PMD::
GrabSingleDepth(Camera<float> &image)
{
  if (!DepthImgF_.IsEmpty())
    {
      image = DepthImgF_;
      return 0;
    }
  return -1;
}

/** Returns the amplitude image. Must be called after GrabSingle() */
int VideoSource_PMD::
GrabSingleModCoeff(Camera<float> &image)
{
  if (!ModCoeffImgF_.IsEmpty())
    {
      image = ModCoeffImgF_;
      return 0;
    }
  return -1;
}

/** Returns the amplitude image. Must be called after GrabSingle() */
int VideoSource_PMD::
GrabSingleIntensity(Camera<float> &image)
{
  if (!IntensityImgF_.IsEmpty())
    {
      if(image.GetStorageType() != IntensityImgF_.GetStorageType()){
        BIASWARNONCE("Images of different storage type!");
        return -1;
      }            
      image = IntensityImgF_;
      return 0;
    }
  return -1;
}

void VideoSource_PMD::
SetGenerateNewUIDs(bool s)
{
  BIASERR("Always generating new UUIDS for PMD images");
  BIASWARNONCE("Not implemented!");
}

void VideoSource_PMD::
EnsureLatestFrame(const bool ensure)
{
  BIASWARN("To be implemented!");
  bEnsureLatestFrame_ = ensure;
}

int VideoSource_PMD::
SetFPNCalibrationOn(bool on)
{
  if (bIsOpen_ || sourcePluginIsOpen_ ) {
    if (on) {
      pmdSourceCommand (handle_, 0, 0, "SetFPNCalibration On");
    } else {
      pmdSourceCommand (handle_, 0, 0, "SetFPNCalibration Off");
    }

    return 0;
  } else {
    BIASERR("device is not opened, call OpenDevice first \n");
    return -1;
  }
}

int VideoSource_PMD::
GetSourceDataSize() {
    if (bIsOpen_ || sourcePluginIsOpen_) {
    return datadesc_.size;
    } else {
        BIASERR("call OpenDevice first" << std::endl);
        return -1;
    }

}