VideoSource_DiskPMD.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_DiskPMD.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_DiskPMD::
VideoSource_DiskPMD() : VideoSource_Disk()
{
  GenerateNewUIDs_=true;
}

BIAS::VideoSource_DiskPMD::
~VideoSource_DiskPMD()
{
}

int BIAS::VideoSource_DiskPMD::
OpenDevice()
{
  BIASERR("use OpenDevice(const vector<string> &FileNames)");
  BIASABORT;
  return -1;
}

#ifdef BIAS_HAVE_XML2

int BIAS::VideoSource_DiskPMD::OpenDevice(string &FileName)
{
  vecImg2D_.clear();
  vecImgDepth_.clear();
  vecImgModCoeff_.clear();
  vecMetaData2D_.clear();
  vecMetaDataDepth_.clear();
  FileNames_.clear();

  // open xml file with image sequence
  BIASDOUT(D_VSO_DISK,"FileName : "<<FileName);

  if (PMDImageIO::LoadFromXML(FileName, vecImg2D_, vecImgDepth_, vecImgModCoeff_,
    vecMetaData2D_, vecMetaDataDepth_)!=0)
  {
    BIASERR("unable to read "<<FileName);
    return -1;
  }
  if (vecImg2D_.size()>0) {
    Width_ = vecMetaData2D_[0].width;
    Height_ = vecMetaData2D_[0].height;
    ColorChannels_ = vecMetaData2D_[0].channels;
    ColorMode_ = vecImg2D_[0].GetColorModel();
    BytesPerPixel_ = vecImg2D_[0].GetDepth();
  } else   {
    BIASERR("No frames in file "<<FileName);
    return -1;
  }

  for (unsigned int i=0; i<vecMetaData2D_.size(); i++) {
    stringstream ss;
    ss << FileHandling::Basename(FileName) << i << FileHandling::Suffix(FileName);
    FileNames_.push_back(ss.str().c_str());
  }

  CompleteInitialized_ = true;
  ActiveFrame_ = 0;

  return 0;
}
#endif

int BIAS::VideoSource_DiskPMD::OpenDevice(const vector<string> &FileNames)
{

  // load first image and initialize some internal variables
  Image<unsigned char> Img2D;
  Image<float> ImgDepth, ImgModCoeff;
  PMDImageMetaData MetaData2D, MetaDataDepth;
  BIASDOUT(D_VSO_DISK,"FileNames[0] : "<<FileNames[0]);

  if (PMDImageIO::Load(FileNames[0],
                         Img2D, ImgDepth, ImgModCoeff,
                         MetaData2D, MetaDataDepth)!=0) {
    BIASERR("unable to read "<<FileNames[0]);
    return -1;
  }

  Width_ = MetaData2D.width;
  Height_ = MetaData2D.height;
  ColorChannels_ = MetaData2D.channels;
  ColorMode_ = Img2D.GetColorModel();
  BytesPerPixel_ = Img2D.GetDepth();

  FileNames_.clear();
  for (vector<string>::const_iterator i = FileNames.begin();
       i != FileNames.end();i++){
    FileNames_.push_back(*i);
  }

  CompleteInitialized_ = true;
  ActiveFrame_ = 0;

  return 0;
}


int BIAS::VideoSource_DiskPMD::GrabSingle(Camera<unsigned char> &image)
{
  BIASDOUT(D_VSO_DISK,"VideoSource_DiskPMD::GrabSingle()");

  if (!Active_) {
    BIASERR("VideoSource_Disk::GrabSingle(): VideoSource Object not active, use PreGrab()");
    return -1;
  }

  if (Grabbing_) {
    BIASERR("VideoSource_Disk::GrabSingle(): VideoSource Object is already grabbing()");
    return -1;
  }

  // Add Metadata to the Camera via Projection Object
  PMDImageMetaData MetaData2D, MetaDataDepth;
  if (vecImg2D_.size()==0) {
    if (ActiveFrame_ == FileNames_.size()){
      BIASERR("VideoSource_DISK: End of input. SetLoop(true) or Rewind()");
      return -1;
    }

    if (PMDImageIO::Load(FileNames_[ActiveFrame_], image, DepthImg_, ModCoeffImg_,
                           MetaData2D, MetaDataDepth)<0) {
      BIASERR("VideoSource_Disk::GrabSingle(): Unable to read image: "
              << FileNames_[ActiveFrame_]);
      return -1;
    }
  } else {
    if (ActiveFrame_ == vecImg2D_.size()){
      BIASERR("VideoSource_DISK: End of input. SetLoop(true) or Rewind()");
      return -1;
    }

    image = Camera<unsigned char>(vecImg2D_[ActiveFrame_]);
    DepthImg_ = Camera<float>(vecImgDepth_[ActiveFrame_]);
    ModCoeffImg_ =  Camera<float>(vecImgModCoeff_[ActiveFrame_]);
    MetaData2D = vecMetaData2D_[ActiveFrame_];
    MetaDataDepth = vecMetaDataDepth_[ActiveFrame_];

  }
  Grabbing_ = true;

  KMatrix K2D = PMDImageProc::KFromMeta(MetaData2D);
  KMatrix Kdepth = PMDImageProc::KFromMeta(MetaDataDepth);

  Pose Pose2D = PMDImageProc::PoseFromMeta(MetaData2D);
  Pose PoseDepth = PMDImageProc::PoseFromMeta(MetaDataDepth);

  Projection Proj2D, ProjDepth;

  vector<double> UndistCoeff2D(4,0.0);
  UndistCoeff2D[0] =MetaData2D.UndistCoeff[0];
  UndistCoeff2D[1] =MetaData2D.UndistCoeff[1];
  UndistCoeff2D[2] =MetaData2D.UndistCoeff[2];
  UndistCoeff2D[3] =MetaData2D.UndistCoeff[3];
  BIAS_ProjParaPersp_DISTORTION_TYPE radDistType=DISTYPE_DEF;
  vector<double> UndistCoeffDepth(4,0.0);
  UndistCoeffDepth[0] =MetaDataDepth.UndistCoeff[0];
  UndistCoeffDepth[1] =MetaDataDepth.UndistCoeff[1];
  UndistCoeffDepth[2] =MetaDataDepth.UndistCoeff[2];
  UndistCoeffDepth[3] =MetaDataDepth.UndistCoeff[3];


  Proj2D.CreatePerspective(Pose2D, K2D,
                           MetaData2D.width, MetaData2D.height,
                           radDistType,UndistCoeff2D);
  ProjDepth.CreatePerspective(PoseDepth, Kdepth,
                              MetaDataDepth.width, MetaDataDepth.height,
                              radDistType,UndistCoeffDepth);
  image.SetProj(Proj2D);
  image.UpdateMetaData();
  image.SetUID(BIAS::UUID::GenerateUUID());
  DepthImg_.SetProj(ProjDepth);
  DepthImg_.UpdateMetaData();
  DepthImg_.SetUID(image.GetUID());
  ModCoeffImg_.SetProj(ProjDepth);
  ModCoeffImg_.UpdateMetaData();
  ModCoeffImg_.SetUID(image.GetUID());

  // set time to _now_
  if (FrameCounterAsTimeStamp_) image.SetTime(ActiveFrame_,0);
  else {
    timeval tv;
    gettimeofday(&tv, NULL);
    image.SetTime(tv.tv_sec,tv.tv_usec);
  }
  if(ActiveFrame_<FileNames_.size()-1){
    BIASDOUT(D_VSO_DISK,"GrabSingle() "<<ActiveFrame_<<"  "<<
             FileNames_[ActiveFrame_]);
    ActiveFrame_++;
  }
  else if (Loop_) {
    ActiveFrame_=0;
    BIASDOUT(D_VSO_DISK,"GrabSingle() restarting with first file");
  }
  else ActiveFrame_++;

  Grabbing_ = false;
  BIASDOUT(D_VSO_DISK,"leaving GrabSingle()");
  // check time and sleep while enough time elapsed
  double secPerFrame=1.0/FramesPerSecond_;

  //struct timeval now;         -- displaced by TimeMeasure
  //double secs=0.0;            --         -"-
  //while (secs<=secPerFrame) { --         -"-
  if(KeepFPS_) {
    timer_.Stop();
    while (timer_.GetRealTime()<=(1E06*secPerFrame)) {
      timer_.Start();
#ifdef   WIN32
      Sleep(1);        //go sleeping for 1 msec
#else  //not WIN32
      usleep(1000);     //go sleeping for 1 msec
#endif //WIN32
      timer_.Stop();

    }
    timer_.Reset();
    timer_.Start();
  }
  //lastTime_=now;   -- displaced by TimeMeasure

  return 0;
}