PMDImageIO.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, Save to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "PMDImageIO.hh"
#include <stdio.h>
#include <math.h>
#include <iostream>

#define UINT16_MAX (65535U)

// depth range in m
#define DEPTH_MAX 65.535f
#define DEPTH_MIN 0
// modulation coefficient range
#define MODCOEFF_MAX 1000
#define MODCOEFF_MIN 0

using namespace std;
using namespace BIAS;

// static
TIFFExtendProc PMDImageIO::ParentExtender = NULL;


// new tiff tag definitions for extrinsic and intrinsic camera parameters
static const TIFFFieldInfo tiffFieldInfo[] =   
  {{ TIFFTAG_PRINCIPALPOINT, -1, -1, TIFF_DOUBLE, FIELD_CUSTOM, true, true, 
     (char*)("PrincipalPoint")},
   { TIFFTAG_FOCALLENGTHX, -1, -1, TIFF_DOUBLE, FIELD_CUSTOM, true, true,
     (char*)("FocalLengthX") },
   { TIFFTAG_ASPECTRATIO, -1, -1, TIFF_DOUBLE, FIELD_CUSTOM, true, true,
     (char*)("AspectRatio") },
   { TIFFTAG_UNDISTORTION, -1, -1, TIFF_DOUBLE, FIELD_CUSTOM, true, true,
     (char*)("BougetUndistortion") },
   { TIFFTAG_CAMERACENTER, -1, -1, TIFF_DOUBLE, FIELD_CUSTOM, true, true, 
     (char*)("CameraCenter") },
   { TIFFTAG_CAMERAROTATION, -1, -1, TIFF_DOUBLE, FIELD_CUSTOM, true, true,
     (char*)("CameraRotation") }
};


#ifndef BIASERR

#ifdef WIN32
#  define FUNCNAME  __FUNCTION__
#else // WIN32
#  define FUNCNAME  __PRETTY_FUNCTION__
#endif // WIN32

#  define BIASERR(arg) { \
  cerr<<std::flush<<"Error in "<<__FILE__<<":"<<__LINE__<<" : "<<FUNCNAME<<std::endl \
  <<"\t"<<arg<<std::endl<<std::flush; \
}

#endif

int PMDImageIO::Save2DImage8Bit_(TIFF *tiffimage, uint8* Img2D,
                                  PMDImageMetaData MetaData2D,
                                  int LinesPerStrip) {

  TIFFSetField(tiffimage, TIFFTAG_IMAGEDESCRIPTION, "2D-Image");
  // We need to set some values for basic tags before we can add any data
  TIFFSetField(tiffimage, TIFFTAG_IMAGEWIDTH, MetaData2D.width);
  TIFFSetField(tiffimage, TIFFTAG_IMAGELENGTH, MetaData2D.height);
  TIFFSetField(tiffimage, TIFFTAG_BITSPERSAMPLE, 8);
  TIFFSetField(tiffimage, TIFFTAG_SAMPLESPERPIXEL, MetaData2D.channels);
  TIFFSetField(tiffimage, TIFFTAG_ROWSPERSTRIP, LinesPerStrip);
  //TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_NONE);
  TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_DEFLATE);
  if (MetaData2D.channels==3) {
    TIFFSetField(tiffimage, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_RGB);
  } else {
    TIFFSetField(tiffimage, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK);
  }
  //TIFFSetField(tiffimage, TIFFTAG_FILLORDER, FILLORDER_MSB2LSB);
  TIFFSetField(tiffimage, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);

  SaveMetaData_(tiffimage, MetaData2D);

  // Save the information to the file
  unsigned char* ipt = Img2D;

  for (unsigned int i=0; 
       i< ceil((double)MetaData2D.height/(double)LinesPerStrip); i++) {
    int bytecount = MetaData2D.width*LinesPerStrip*MetaData2D.channels;
    if ((i+1)*LinesPerStrip>MetaData2D.height) {
      bytecount = (MetaData2D.height-i*LinesPerStrip)*
        MetaData2D.width*MetaData2D.channels;
    }
    TIFFReadEncodedStrip(tiffimage, i, ipt, bytecount);
    ipt+=MetaData2D.width*LinesPerStrip*MetaData2D.channels;
  }
  return 0;
}


int PMDImageIO::Save2DImage16Bit_(TIFF *tiffimage, uint16* Img2D,
                                   PMDImageMetaData MetaData2D,
                                   int LinesPerStrip) {

  TIFFSetField(tiffimage, TIFFTAG_IMAGEDESCRIPTION, "2D-Image");
  // We need to set some values for basic tags before we can add any data
  TIFFSetField(tiffimage, TIFFTAG_IMAGEWIDTH, MetaData2D.width);
  TIFFSetField(tiffimage, TIFFTAG_IMAGELENGTH, MetaData2D.height);
  TIFFSetField(tiffimage, TIFFTAG_BITSPERSAMPLE, 16);
  TIFFSetField(tiffimage, TIFFTAG_SAMPLESPERPIXEL, MetaData2D.channels);
  TIFFSetField(tiffimage, TIFFTAG_ROWSPERSTRIP, LinesPerStrip);
  //TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_NONE);
  TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_DEFLATE);
  if (MetaData2D.channels==3) {
    TIFFSetField(tiffimage, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_RGB);
  } else {
    TIFFSetField(tiffimage, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK);
  }
  //TIFFSetField(tiffimage, TIFFTAG_FILLORDER, FILLORDER_MSB2LSB);
  TIFFSetField(tiffimage, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);

  SaveMetaData_(tiffimage, MetaData2D);

  // Save the information to the file
  unsigned char* ipt = (unsigned char*) Img2D;

  for (unsigned int i=0; 
       i< ceil((double)MetaData2D.height/(double)LinesPerStrip); i++) {
    int bytecount = MetaData2D.width*LinesPerStrip*MetaData2D.channels*2;
    if ((i+1)*LinesPerStrip>MetaData2D.height) {
      bytecount = (MetaData2D.height-i*LinesPerStrip)*
        MetaData2D.width*MetaData2D.channels*2;
    }
    TIFFWriteEncodedStrip(tiffimage, i, ipt, bytecount);
    ipt+=MetaData2D.width*LinesPerStrip*MetaData2D.channels*2;
  }
  return 0;
}


int PMDImageIO::SaveDepthImage_(TIFF *tiffimage, uint16* ImgDepth, 
                                 PMDImageMetaData MetaDataDepth,
                                 int LinesPerStrip) {

  TIFFSetField(tiffimage, TIFFTAG_IMAGEDESCRIPTION, "Depth-Image");
  // We need to set some values for basic tags before we can add any data
  TIFFSetField(tiffimage, TIFFTAG_IMAGEWIDTH, MetaDataDepth.width);
  TIFFSetField(tiffimage, TIFFTAG_IMAGELENGTH, MetaDataDepth.height);
  TIFFSetField(tiffimage, TIFFTAG_BITSPERSAMPLE, 16);
  TIFFSetField(tiffimage, TIFFTAG_SAMPLESPERPIXEL, 1);
  TIFFSetField(tiffimage, TIFFTAG_ROWSPERSTRIP, LinesPerStrip);
  //TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_NONE);
  TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_DEFLATE);
  TIFFSetField(tiffimage, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK);
  //TIFFSetField(tiffimage, TIFFTAG_FILLORDER, FILLORDER_MSB2LSB);
  TIFFSetField(tiffimage, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);

  SaveMetaData_(tiffimage, MetaDataDepth);

  // Save the information to the file
  unsigned char* ipt = (unsigned char*) ImgDepth;
  for (unsigned int i=0; 
       i< ceil((double)MetaDataDepth.height/(double)LinesPerStrip); i++) {
    int bytecount = MetaDataDepth.width*LinesPerStrip*
      MetaDataDepth.channels*2;
    if ((i+1)*LinesPerStrip>MetaDataDepth.height) {
      bytecount = (MetaDataDepth.height-i*LinesPerStrip)*
        MetaDataDepth.width*MetaDataDepth.channels*2;
    }
    TIFFWriteEncodedStrip(tiffimage, i, ipt, bytecount);
    ipt+=MetaDataDepth.width*LinesPerStrip*
      MetaDataDepth.channels*2;
  }
  return 0;
}


int PMDImageIO::SaveModCoeffImage_(TIFF *tiffimage, uint16* ImgModCoeff, 
                                    PMDImageMetaData MetaDataDepth,
                                    int LinesPerStrip) {

  TIFFSetField(tiffimage, TIFFTAG_IMAGEDESCRIPTION, 
               "Modulation-Coefficients");
  // We need to set some values for basic tags before we can add any data
  TIFFSetField(tiffimage, TIFFTAG_IMAGEWIDTH, MetaDataDepth.width);
  TIFFSetField(tiffimage, TIFFTAG_IMAGELENGTH, MetaDataDepth.height);
  TIFFSetField(tiffimage, TIFFTAG_BITSPERSAMPLE, 16);
  TIFFSetField(tiffimage, TIFFTAG_SAMPLESPERPIXEL, 1);
  TIFFSetField(tiffimage, TIFFTAG_ROWSPERSTRIP, LinesPerStrip);
  //TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_NONE);
  TIFFSetField(tiffimage, TIFFTAG_COMPRESSION, COMPRESSION_DEFLATE);
  TIFFSetField(tiffimage, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK);
  //TIFFSetField(tiffimage, TIFFTAG_FILLORDER, FILLORDER_MSB2LSB);
  TIFFSetField(tiffimage, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);

  // The metadata of the modulatioin coefficient is the same as for 
  // the depth image
  SaveMetaData_(tiffimage, MetaDataDepth);

  // Save the information to the file
  unsigned char* ipt = (unsigned char*) ImgModCoeff;
  for (unsigned int i=0; 
       i< ceil((double)MetaDataDepth.height/(double)LinesPerStrip); i++) {
    int bytecount = MetaDataDepth.width*LinesPerStrip*
      MetaDataDepth.channels*2;
    if ((i+1)*LinesPerStrip>MetaDataDepth.height) {
      bytecount = (MetaDataDepth.height-i*LinesPerStrip)*
        MetaDataDepth.width*MetaDataDepth.channels*2;
    }
    TIFFWriteEncodedStrip(tiffimage, i, ipt, bytecount);
    ipt+=MetaDataDepth.width*LinesPerStrip*
      MetaDataDepth.channels*2;
  }
  return 0;
}


int PMDImageIO::Save(const std::string& FileName, 
                     uint8* Img2D, uint16* ImgDepth, 
                     uint16* ImgModCoeff,
                     const PMDImageMetaData &MetaData2D,
                     const PMDImageMetaData &MetaDataDepth) {

  // Open the TIFF file
  TIFF *tiffimage;
  int LinesPerStrip=10;

  /* Set up the callback */
  NewTagsInitialize_(); 

  if((tiffimage = TIFFOpen(FileName.c_str(), "w")) == NULL){
    BIASERR("Could not open image for writing:" << FileName);
    return -1;
  }

  // Save the rgb or grey image
  if (Img2D!=NULL) {
    Save2DImage8Bit_(tiffimage, Img2D, MetaData2D, LinesPerStrip);
  }

  // Save the depth image
  if (ImgDepth!=NULL) {
    if (Img2D!=NULL) TIFFWriteDirectory(tiffimage);
    SaveDepthImage_(tiffimage, ImgDepth, MetaDataDepth, LinesPerStrip);
  }

  // Save the modulation coefficients
  // No modulation coefficients without a depth image
  if (ImgDepth!=NULL && ImgModCoeff!=NULL) {
  
    TIFFWriteDirectory(tiffimage);
    SaveModCoeffImage_(tiffimage, ImgModCoeff, MetaDataDepth, LinesPerStrip);
  }

  // Close the file
  TIFFClose(tiffimage);
  return 0;
}


int PMDImageIO::Save(const std::string& FileName, 
                     uint16* Img2D, uint16* ImgDepth, 
                     uint16* ImgModCoeff,
                     const PMDImageMetaData &MetaData2D,
                     const PMDImageMetaData &MetaDataDepth) {

  // Open the TIFF file
  TIFF *tiffimage;
  int LinesPerStrip=10;

  /* Set up the callback */
  NewTagsInitialize_(); 

  if((tiffimage = TIFFOpen(FileName.c_str(), "w")) == NULL){
    BIASERR("Could not open image for writing:" << FileName);
    return -1;
  }

  // Save the rgb or grey image
  if (Img2D!=NULL) {
    Save2DImage16Bit_(tiffimage, Img2D, MetaData2D, LinesPerStrip);
  }

  // Save the depth image
  if (ImgDepth!=NULL) {
    if (Img2D!=NULL) TIFFWriteDirectory(tiffimage);
    SaveDepthImage_(tiffimage, ImgDepth, MetaDataDepth, LinesPerStrip);
  }

  // Save the modulation coefficients
  // No modulation coefficients without a depth image
  if (ImgDepth!=NULL && ImgModCoeff!=NULL) {
  
    TIFFWriteDirectory(tiffimage);
    SaveModCoeffImage_(tiffimage, ImgModCoeff, MetaDataDepth, LinesPerStrip);
  }

  // Close the file
  TIFFClose(tiffimage);
  return 0;
}


// Load the rgb or grey image -----------------
int PMDImageIO::Load2DImage8Bit_(TIFF *tiffimage, 
                                 uint8* &Img2D, 
                                 PMDImageMetaData &MetaData2D) {
  
  uint16 bps, photo;

  if((TIFFGetField(tiffimage, TIFFTAG_BITSPERSAMPLE, &bps) == 0) || 
     (bps != 8)){
    BIASERR("Bits per Sample != 8!");
    return -1;
  }
    
  if(TIFFGetField(tiffimage, TIFFTAG_PHOTOMETRIC, &photo) == 0 || 
     (photo != PHOTOMETRIC_RGB && photo != PHOTOMETRIC_MINISBLACK)){
    BIASERR("Image has the wrong photometric interpretation! Trying PHOTOMETRIC_MINISBLACK.");
    photo = PHOTOMETRIC_MINISBLACK;
    // return -1;
  }
      
  LoadMetaData_(tiffimage, MetaData2D);

  if ((MetaData2D.channels!=3) && (MetaData2D.channels!=1)) {
    BIASERR("Wrong number of channels in 2D image!");
    return -1;        
  }
 
  // Load in the possibly multiple strips
  tsize_t stripSize = TIFFStripSize (tiffimage);
  int stripMax = TIFFNumberOfStrips (tiffimage);
   
  unsigned char *ipt = new unsigned char[MetaData2D.height*
                                         MetaData2D.width*
                                         MetaData2D.channels];
  Img2D = ipt;

  for (int i = 0; i < stripMax; i++){
    if (i==stripMax-1) {
      stripSize = MetaData2D.height*MetaData2D.width*
        MetaData2D.channels-i*stripSize;
    }
    if(TIFFReadEncodedStrip(tiffimage, i, ipt, stripSize)<0){
      BIASERR("Load error on input strip number "<< i);
      return -1;
    }
    ipt += stripSize;
  }
  return 0;
}


  // Load the rgb or grey image -----------------
int PMDImageIO::Load2DImage16Bit_(TIFF *tiffimage, 
                                  uint16* &Img2D, 
                                  PMDImageMetaData &MetaData2D){

  uint16 bps, photo;

  if((TIFFGetField(tiffimage, TIFFTAG_BITSPERSAMPLE, &bps) == 0) || 
     (bps != 16)){
    BIASERR("Bits per Sample != 16!");
    return -1;
  }
    
  if(TIFFGetField(tiffimage, TIFFTAG_PHOTOMETRIC, &photo) == 0 || 
     (photo != PHOTOMETRIC_RGB && photo != PHOTOMETRIC_MINISBLACK)){
    BIASERR("Image has the wrong photometric interpretation!");
    return -1;
  }
      
  LoadMetaData_(tiffimage, MetaData2D);

  if ((MetaData2D.channels!=3) && (MetaData2D.channels!=1)) {
    BIASERR("Wrong number of channels in 2D image!");
    return -1;        
  }
 
  // Load in the possibly multiple strips
  tsize_t stripSize = TIFFStripSize (tiffimage);
  int stripMax = TIFFNumberOfStrips (tiffimage);
   
  unsigned char *ipt = new unsigned char[MetaData2D.height*
                                         MetaData2D.width*
                                         MetaData2D.channels*2];
  Img2D = (uint16*)ipt;

  for (int i = 0; i < stripMax; i++){
    if (i==stripMax-1) {
      stripSize = MetaData2D.height*MetaData2D.width*
        MetaData2D.channels-i*stripSize;
    }
    if(TIFFReadEncodedStrip(tiffimage, i, ipt, stripSize)<0){
      BIASERR("Load error on input strip number "<< i);
      return -1;
    }
    ipt += stripSize;
  }
  return 0;
}

// Load the depth-image
int PMDImageIO::LoadDepthImage_(TIFF *tiffimage, 
                                uint16* &ImgDepth, 
                                PMDImageMetaData &MetaDataDepth) {

  uint16 dbps, dphoto;

  if((TIFFGetField(tiffimage, TIFFTAG_BITSPERSAMPLE, &dbps) == 0) || 
     (dbps != 16)){
    BIASERR("Bits per Sample != 16!");
    return -1;
  }

  if(TIFFGetField(tiffimage, TIFFTAG_PHOTOMETRIC, &dphoto) == 0 || 
     (dphoto != PHOTOMETRIC_MINISBLACK)){
    BIASERR("Depth Image has the wrong photometric interpretation!");
    return -1;
  }

  LoadMetaData_(tiffimage, MetaDataDepth);
 
  if (MetaDataDepth.channels!=1) {
    BIASERR("Wrong number of channels in Depth image!");
    return -1;        
  }

  // Load in the possibly multiple strips
  tsize_t stripSize = TIFFStripSize (tiffimage);
  int stripMax = TIFFNumberOfStrips (tiffimage);

  unsigned char* ipt = new unsigned char[MetaDataDepth.height*
                                         MetaDataDepth.width*
                                         MetaDataDepth.channels*2];
  ImgDepth = (uint16*)ipt;
  for (int i = 0; i < stripMax; i++){
    if (i==stripMax-1) {
      stripSize = MetaDataDepth.height*MetaDataDepth.width*
        MetaDataDepth.channels*2-i*stripSize;
    }
    if(TIFFReadEncodedStrip(tiffimage, i, ipt, stripSize)<0){
      BIASERR("Load error on input strip number "<< i);
      return -1;
    }
    ipt += stripSize;
  }
  return 0;
}

// Load the Modulation-Coefficients
int PMDImageIO::LoadModCoeffImage_(TIFF *tiffimage, 
                                   uint16* &ImgModCoeff, 
                                   PMDImageMetaData MetaDataDepth) {

  uint16 dbps, dphoto;

  if((TIFFGetField(tiffimage, TIFFTAG_BITSPERSAMPLE, &dbps) == 0) || 
     (dbps != 16)){
    BIASERR("Bits per Sample != 16!");
    return -1;
  }

  if(TIFFGetField(tiffimage, TIFFTAG_PHOTOMETRIC, &dphoto) == 0 || 
     (dphoto != PHOTOMETRIC_MINISBLACK)){
    BIASERR("Modulation Coefficients have the wrong photometric"
            << "interpretation!");
    return -1;
  }

  LoadMetaData_(tiffimage, MetaDataDepth);
 
  if (MetaDataDepth.channels!=1) {
    BIASERR("Wrong number of channels in Modulation Coefficients image!");
    return -1;        
  }

  // Load in the possibly multiple strips
  tsize_t stripSize = TIFFStripSize (tiffimage);
  int stripMax = TIFFNumberOfStrips (tiffimage);

  unsigned char* ipt = new unsigned char[MetaDataDepth.height*
                                         MetaDataDepth.width*
                                         MetaDataDepth.channels*2];
  ImgModCoeff = (uint16*)ipt;
  for (int i = 0; i < stripMax; i++){
    if (i==stripMax-1) {
      stripSize = MetaDataDepth.height*MetaDataDepth.width*
        MetaDataDepth.channels*2-i*stripSize;
    }
    if(TIFFReadEncodedStrip(tiffimage, i, ipt, stripSize)<0){
      BIASERR("Load error on input strip number "<< i);
      return -1;
    }
    ipt += stripSize;
  }
  return 0;
}


int PMDImageIO::Load(const std::string& FileName, 
                     uint16* &Img2D, uint16* &ImgDepth, 
                     uint16* &ImgModCoeff,
                     PMDImageMetaData &MetaData2D,
                     PMDImageMetaData &MetaDataDepth){
  // reset image pointer
  Img2D=NULL;
  ImgDepth=NULL;
  ImgModCoeff=NULL;

  // Open the TIFF file
  TIFF *tiffimage;

  /* Set up the callback */
  NewTagsInitialize_(); 

  if((tiffimage = TIFFOpen(FileName.c_str(), "r")) == NULL){
    BIASERR("Could not open image for Loading:" << FileName);
    return -1;
  }

  do {
    char* descr;
    if (TIFFGetField(tiffimage, TIFFTAG_IMAGEDESCRIPTION, &descr) == 0) {
      BIASERR("Image description is missing!");
      return -1;
    }
 
    if(strcmp(descr, "2D-Image") == 0) {
      if (Load2DImage16Bit_(tiffimage, Img2D, MetaData2D)<0) 
        return -1;
    }  
 
    if(strcmp(descr, "Depth-Image") == 0) {
      if (LoadDepthImage_(tiffimage, ImgDepth, MetaDataDepth)<0)
          return -2;
    }

    if(strcmp(descr, "Modulation-Coefficients") == 0) {
      if (LoadModCoeffImage_(tiffimage, ImgModCoeff, MetaDataDepth)<0) 
        return -3;
    }

  } while (TIFFReadDirectory(tiffimage)!=0);

  // Close the file
  TIFFClose(tiffimage);
  return 0;
}


int PMDImageIO::Load(const std::string& FileName, 
                     uint8* &Img2D, uint16* &ImgDepth, 
                     uint16* &ImgModCoeff,
                     PMDImageMetaData &MetaData2D,
                     PMDImageMetaData &MetaDataDepth){
  
  // reset image pointer
  Img2D=NULL;
  ImgDepth=NULL;
  ImgModCoeff=NULL;

  // Open the TIFF file
  TIFF *tiffimage;

  /* Set up the callback */
  NewTagsInitialize_();

  if((tiffimage = TIFFOpen(FileName.c_str(), "r")) == NULL){
    BIASERR("Could not open image for Loading:" << FileName);
    return -1;
  }

  do {
    char* descr;
    if (TIFFGetField(tiffimage, TIFFTAG_IMAGEDESCRIPTION, &descr) == 0) {
      BIASERR("Image description is missing!");
      return -1;
    }
 
    if(strcmp(descr, "2D-Image") == 0) {
      if (Load2DImage8Bit_(tiffimage, Img2D, MetaData2D)<0) 
        return -1;
    }  
 
    if(strcmp(descr, "Depth-Image") == 0) {
      if (LoadDepthImage_(tiffimage, ImgDepth, MetaDataDepth)<0)
          return -2;
    }

    if(strcmp(descr, "Modulation-Coefficients") == 0) {
      if (LoadModCoeffImage_(tiffimage, ImgModCoeff, MetaDataDepth)<0) 
        return -3;
    }

  } while (TIFFReadDirectory(tiffimage)!=0);

  // Close the file
  TIFFClose(tiffimage);
  return 0;
}


#ifdef BIAS_HAVE_XML2

int PMDImageIO::LoadXML(const std::string& FileName, 
                        uint16* &Img2D, uint16* &ImgDepth, 
                        uint16* &ImgModCoeff,
                        PMDImageMetaData &MetaData2D,
                        PMDImageMetaData &MetaDataDepth){
  
  // Open the XML file
  XMLIO myXML;
  xmlNodePtr rootNode;
  rootNode = myXML.read(FileName);
  if (rootNode==NULL) {
    BIASERR("PMD-XML-File not found or parse error: " << FileName);
    return -1;
  }
  if (myXML.getNodeName(rootNode)!="frame" && myXML.getNodeName(rootNode)!="sequence") {
    BIASERR("Root node has to be \"frame\" or \"sequence\"");
    return -1;
  }
  if (myXML.getNodeName(rootNode)=="sequence") {
    rootNode = myXML.getChild(rootNode, "frame");
    if (rootNode==NULL) {
      BIASERR("Root node \"sequence\" has no child \"frame\"");
      return -1;
    }
  }
  return LoadXML(myXML, rootNode, Img2D, ImgDepth, ImgModCoeff, MetaData2D, MetaDataDepth);
  
}

int PMDImageIO::LoadXML(XMLIO &myXML, xmlNodePtr &rootNode, 
                        uint16* &Img2D, uint16* &ImgDepth, 
                        uint16* &ImgModCoeff,
                        PMDImageMetaData &MetaData2D,
                        PMDImageMetaData &MetaDataDepth){
   // reset image pointer
  Img2D=NULL;
  ImgDepth=NULL;
  ImgModCoeff=NULL;

  unsigned int width, height;
  double aspectratio; 
  
  width = myXML.getAttributeValueInt(rootNode, "width");
  height = myXML.getAttributeValueInt(rootNode, "height");
  aspectratio = myXML.getAttributeValueDouble(rootNode, "aspect");
  
  xmlNodePtr  childNode;
  vector<int> content;
  childNode = myXML.getChild(rootNode, "grayscale");
  if (childNode!=NULL) {
    Img2D = new uint16[width*height];
    uint16* pI = Img2D; 
    content = myXML.getNodeContentVectorInt(childNode);
    if (content.size()!=width*height) {
      BIASERR("Error in XML-File!!");
      return -1;
    }
    for (unsigned int i=0; i<content.size(); i++) {
      *pI = (uint16)content[i];  
      pI++;
    }
  }

  childNode = myXML.getChild(rootNode, "distance");
  if (childNode!=NULL) {
    ImgDepth = new uint16[width*height];
    uint16* pI = ImgDepth; 
    content = myXML.getNodeContentVectorInt(childNode);  
    if (content.size()!=width*height) {
      BIASERR("Error in XML-File!!");
      return -1;
    }
    for (unsigned int i=0; i<content.size(); i++) {
      *pI = (uint16)content[i];  
      pI++;
    }
  }

  childNode = myXML.getChild(rootNode, "amplitude");
  if (childNode!=NULL) {
    ImgModCoeff = new uint16[width*height];
    uint16* pI = ImgModCoeff; 
    content = myXML.getNodeContentVectorInt(childNode);  
    if (content.size()!=width*height) {
      BIASERR("Error in XML-File!!");
      return -1;
    }
    for (unsigned int i=0; i<content.size(); i++) {
      *pI = (uint16)content[i];  
      pI++;
    }
  }

  MetaData2D.width = MetaDataDepth.width = width;
  MetaData2D.height = MetaDataDepth.height = height;
  MetaData2D.channels = MetaDataDepth.channels = 1;
  MetaData2D.PrincipalX = MetaDataDepth.PrincipalX = (double)width/2.0;
  MetaData2D.PrincipalY = MetaDataDepth.PrincipalY = (double)height/2.0;
  MetaData2D.FocallengthX = MetaDataDepth.FocallengthX = 1;
  MetaData2D.AspectRatio = MetaDataDepth.AspectRatio = 1/(aspectratio/((double)width/(double)height));
  MetaData2D.UndistCoeff[0] = MetaDataDepth.UndistCoeff[0] = 0;
  MetaData2D.UndistCoeff[1] = MetaDataDepth.UndistCoeff[1] = 0;
  MetaData2D.UndistCoeff[2] = MetaDataDepth.UndistCoeff[2] = 0;
  MetaData2D.UndistCoeff[3] = MetaDataDepth.UndistCoeff[3] = 0;
  MetaData2D.CameraCenter[0] = MetaDataDepth.CameraCenter[0] = 0;
  MetaData2D.CameraCenter[1] = MetaDataDepth.CameraCenter[1] = 0;
  MetaData2D.CameraCenter[2] = MetaDataDepth.CameraCenter[2] = 0;
  MetaData2D.CameraRotation[0] = MetaDataDepth.CameraRotation[0] = 0;
  MetaData2D.CameraRotation[1] = MetaDataDepth.CameraRotation[1] = 0;
  MetaData2D.CameraRotation[2] = MetaDataDepth.CameraRotation[2] = 0;

  return 0;
} 



#endif  // BIAS_HAVE_XML2


int PMDImageIO::Save(const std::string& FileName, 
                     Image<unsigned char> &Img2D, 
                     Image<float> &ImgDepth, 
                     Image<float> &ImgModCoeff, 
                     PMDImageMetaData &MetaData2D,
                     PMDImageMetaData &MetaDataDepth) {

  uint8* data2D=NULL;
  uint16* dataDepth=NULL;
  uint16* dataModCoeff=NULL;

  if (Img2D.IsEmpty()) {
    BIASERR("2D-Data is necessary!");
    return -1;
  }

  MetaData2D.width = Img2D.GetWidth();
  MetaData2D.height = Img2D.GetHeight();
  MetaData2D.channels = Img2D.GetChannelCount();

  if (Img2D.GetStorageType()!=ImageBase::ST_unsignedchar) {
    BIASERR("The RGB or grey image has the wrong storage type, has to be" << 
            "ST_unsignedchar");
    return -1;    
  }
  
  data2D = (uint8*) Img2D.GetImageData();

  if (!ImgDepth.IsEmpty()) {

    MetaDataDepth.width = ImgDepth.GetWidth();
    MetaDataDepth.height = ImgDepth.GetHeight();
    MetaDataDepth.channels = ImgDepth.GetChannelCount();
    if (ImgDepth.GetStorageType()!=ImageBase::ST_float) {
      BIASERR("The depth image has the wrong storage type, has to be" << 
        "ST_float");
      return -1;    
    }

    unsigned int i;
    dataDepth = new uint16[MetaDataDepth.width*MetaDataDepth.height];
    uint16* pDataDepth = dataDepth;
    float* pDataDepthf = ImgDepth.GetImageData();
    for (i=0; i<MetaDataDepth.width*MetaDataDepth.height; 
      i++, pDataDepthf++, pDataDepth++) {
      *pDataDepth =DepthConvert_(*pDataDepthf);
      }

      if (!ImgModCoeff.IsEmpty()) {
        if (ImgModCoeff.GetStorageType()!=ImageBase::ST_float) {
          BIASERR("The modulation coefficients image has the wrong storage" <<
            "type, has to be ST_float");
          return -1;    
        }
        if (MetaDataDepth.width != ImgModCoeff.GetWidth() ||
          MetaDataDepth.height != ImgModCoeff.GetHeight() ||
          MetaDataDepth.channels != ImgModCoeff.GetChannelCount()) {
            BIASERR("The modulation coefficients image must have same size " <<
              "and channels as the depth image");
            return -1;    
          }

          dataModCoeff = new uint16[MetaDataDepth.width*MetaDataDepth.height];
          uint16* pDataModCoeff = dataModCoeff;
          float* pDataModCoefff = ImgModCoeff.GetImageData();
          for (i=0; i<MetaDataDepth.width*MetaDataDepth.height; 
            i++, pDataModCoefff++, pDataModCoeff++) {
            *pDataModCoeff = ModCoeffConvert_(*pDataModCoefff);
            }
      }
  }
  PMDImageIO::Save(FileName, data2D, dataDepth, dataModCoeff,
                   MetaData2D, MetaDataDepth);

  if (dataDepth!=NULL) delete [] dataDepth;
  if (dataModCoeff!=NULL) delete [] dataModCoeff;

  return 0;

}


int PMDImageIO::Load(const std::string& FileName, 
                     Image<unsigned char> &Img2D, 
                     Image<float> &ImgDepth, 
                     Image<float> &ImgModCoeff, 
                     PMDImageMetaData &MetaData2D, 
                     PMDImageMetaData &MetaDataDepth) {

#ifdef BIAS_HAVE_XML2
  if (strstr(FileName.c_str(),".xml")!=NULL) {
    return PMDImageIO::LoadFromXML(FileName, Img2D, ImgDepth, ImgModCoeff, 
                                     MetaData2D, MetaDataDepth);
  } else {
#endif
    return PMDImageIO::LoadFromTIFF(FileName, Img2D, ImgDepth, ImgModCoeff, 
                                      MetaData2D, MetaDataDepth);
#ifdef BIAS_HAVE_XML2
  }
#endif

}


#ifdef BIAS_HAVE_XML2

int PMDImageIO::LoadFromXML(const std::string& FileName, 
                              Image<unsigned char> &Img2D, 
                              Image<float> &ImgDepth, 
                              Image<float> &ImgModCoeff, 
                              PMDImageMetaData &MetaData2D, 
                              PMDImageMetaData &MetaDataDepth) {

  uint8* data2d=NULL;
  uint16* data2d16=NULL;
  uint16* dataDepth=NULL;
  uint16* dataModCoeff=NULL;

  if (PMDImageIO::LoadXML(FileName, data2d16, dataDepth, dataModCoeff,
                          MetaData2D, MetaDataDepth)<0) {
    BIASERR("Could not Load XML-image.");
    return -1;
  } else {
    Grey16to8Bit_(data2d16, data2d, 
                  MetaData2D.width*MetaData2D.height*MetaData2D.channels);
    return PostprocessData_(data2d, dataDepth, dataModCoeff, 
                            Img2D, ImgDepth, ImgModCoeff, 
                            MetaData2D, MetaDataDepth);
  }
}     


int PMDImageIO::LoadFromXML(const std::string& FileName, 
                              vector<Image<unsigned char> > &vecImg2D, 
                              vector<Image<float> > &vecImgDepth, 
                              vector<Image<float> > &vecImgModCoeff, 
                              vector<PMDImageMetaData> &vecMetaData2D, 
                              vector<PMDImageMetaData> &vecMetaDataDepth) 
{

  uint8* data2d=NULL;
  uint16* data2d16=NULL;
  uint16* dataDepth=NULL;
  uint16* dataModCoeff=NULL;

  Image<unsigned char> Img2D; 
  Image<float> ImgDepth; 
  Image<float> ImgModCoeff;
  PMDImageMetaData MetaData2D;
  PMDImageMetaData MetaDataDepth;

  // Open the XML file
  XMLIO myXML;
  xmlNodePtr rootNode, childNode, tempChildNode;
  rootNode = myXML.read(FileName);
  if (rootNode==NULL) {
    BIASERR("PMD-XML-File not found or parse error: " << FileName);
    return -1;
  }
  if (myXML.getNodeName(rootNode)=="sequence") {
    childNode = myXML.getFirstChild(rootNode);
    while (childNode!=NULL) {
      if (myXML.getNodeName(childNode)=="frame") {
        tempChildNode = childNode;
        if (PMDImageIO::LoadXML(myXML, tempChildNode, data2d16, dataDepth, dataModCoeff,
                                MetaData2D, MetaDataDepth)<0) 
        {
          BIASERR("Could not Load XML-image.");
          return -1;
        } else {
          Grey16to8Bit_(data2d16, data2d, 
                        MetaData2D.width*MetaData2D.height*MetaData2D.channels);
          if (PostprocessData_(data2d, dataDepth, dataModCoeff, 
                                             Img2D, ImgDepth, ImgModCoeff, 
                                             MetaData2D, MetaDataDepth)<0) 
          {
            BIASERR("Error writing images to BIAS::Image.");
            return -1;
          }
          vecImg2D.push_back(Img2D);
          vecImgDepth.push_back(ImgDepth);
          vecImgModCoeff.push_back(ImgModCoeff);
          vecMetaData2D.push_back(MetaData2D);
          vecMetaDataDepth.push_back(MetaDataDepth);
        }
      }
      childNode = myXML.getNextChild(childNode);
    }
  } else {
    if (myXML.getNodeName(rootNode)=="frame") {
      if (LoadFromXML(FileName, Img2D, ImgDepth, ImgModCoeff,
                      MetaData2D,  MetaDataDepth))
      {
        BIASERR("Could not Load XML-image.");
        return -1;
      }
      vecImg2D.push_back(Img2D);
      vecImgDepth.push_back(ImgDepth);
      vecImgModCoeff.push_back(ImgModCoeff);
      vecMetaData2D.push_back(MetaData2D);
      vecMetaDataDepth.push_back(MetaDataDepth);
    } else {
      BIASERR("Root node has to be \"frame\" or \"sequence\"");
      return -1;
    }
  }
  return 0;
}


#endif


int PMDImageIO::LoadFromTIFF(const std::string& FileName, 
                               Image<unsigned char> &Img2D, 
                               Image<float> &ImgDepth, 
                               Image<float> &ImgModCoeff, 
                               PMDImageMetaData &MetaData2D, 
                               PMDImageMetaData &MetaDataDepth) {
                 
  uint8* data2d=NULL;
  uint16* dataDepth=NULL;
  uint16* dataModCoeff=NULL;

  if (PMDImageIO::Load(FileName, data2d, dataDepth, dataModCoeff,
      MetaData2D, MetaDataDepth)<0) {
      BIASERR("Could not Load TIFF-image.");
      return -1;
  }
  return PostprocessData_(data2d, dataDepth, dataModCoeff, 
                                        Img2D, ImgDepth, ImgModCoeff, 
                                        MetaData2D, MetaDataDepth);
}


int PMDImageIO::LoadFromTXT(int width, int height,
                              const std::string& FileName2D, 
                              const std::string& FileNameDepth, 
                              const std::string& FileNameModCoeff, 
                              Image<unsigned char> &Img2D, 
                              Image<float> &ImgDepth, 
                              Image<float> &ImgModCoeff, 
                              PMDImageMetaData &MetaDataDepth) {

  // open file streams
  ifstream in2D(FileName2D.c_str());
  if (!in2D) {
    BIASERR("Error opening 2D file:"<<FileName2D);
    return -1;
  }
  ifstream inDepth(FileNameDepth.c_str());
  if (!inDepth) {
    BIASERR("Error opening Depth file:"<<FileNameDepth);
    return -1;
  }
  ifstream inModCoeff(FileNameModCoeff.c_str());
  if (!inModCoeff) {
    BIASERR("Error opening ModCoeff file:"<<FileNameModCoeff);
    return -1;
  }
  
  uint8* data2d = NULL;
  uint16* data2d16 = new uint16[width*height];
  uint16* dataDepth = new uint16[width*height];
  uint16* dataModCoeff = new uint16[width*height];
  
  double d;
  for (int i=0; i<width*height; i++) {
    in2D >> d;
    data2d16[i] = uint16(d);
    inDepth >> d;
    dataDepth[i] = uint16(d);
    inModCoeff >> d;
    dataModCoeff[i] = uint16(d);
  }
  MetaDataDepth.width = width;
  MetaDataDepth.height = height;
  MetaDataDepth.channels = 1;
  MetaDataDepth.PrincipalX = (double)width/2.0;
  MetaDataDepth.PrincipalY = (double)height/2.0;
  MetaDataDepth.FocallengthX = 1;
  MetaDataDepth.AspectRatio = 1;
  MetaDataDepth.UndistCoeff[0] = 0;
  MetaDataDepth.UndistCoeff[1] = 0;
  MetaDataDepth.UndistCoeff[2] = 0;
  MetaDataDepth.UndistCoeff[3] = 0;
  MetaDataDepth.CameraCenter[0] = 0;
  MetaDataDepth.CameraCenter[1] = 0;
  MetaDataDepth.CameraCenter[2] = 0;
  MetaDataDepth.CameraRotation[0] = 0;
  MetaDataDepth.CameraRotation[1] = 0;
  MetaDataDepth.CameraRotation[2] = 0;
  Grey16to8Bit_(data2d16, data2d, 
                width*height);
  PostprocessData_(data2d, dataDepth, dataModCoeff, 
                   Img2D, ImgDepth, ImgModCoeff, 
                   MetaDataDepth, MetaDataDepth);  

  //delete [] data2d;
  //delete [] data2d16;
  //delete [] dataDepth;
  //delete [] dataModCoeff;

  return 0;

}

int PMDImageIO::GetBitRep(const std::string& FileName) {
  TIFF *tiffimage;

  if((tiffimage = TIFFOpen(FileName.c_str(), "r")) == NULL){
    BIASERR("Could not open image for Loading:" << FileName);
    return -1;
  }

  do {
    char* descr;
    if (TIFFGetField(tiffimage, TIFFTAG_IMAGEDESCRIPTION, &descr) == 0) {
      BIASERR("Image description is missing!");
      return -1;
    }
    if(strcmp(descr, "2D-Image") == 0) {
      uint16 bps;
      if (TIFFGetField(tiffimage, TIFFTAG_BITSPERSAMPLE, &bps)<0) {
        return -1;
      } else {
        return bps;
      }
    }  
  } while (TIFFReadDirectory(tiffimage)!=0);

  return 0;
}


void PMDImageIO::Grey16to8Bit_(uint16* src, uint8* &dst, 
                                 unsigned int size)
{
  dst = new unsigned char[size];
  uint16 min = 65535;
  uint16 max = 0;
  uint16* p16 = src;
  for (unsigned int i=0; i<size; i++) {
    if ((*p16)<min) min=*p16;
    if ((*p16)>max) max=*p16;
    p16++;
  }
  p16 = src;
  uint8* p8 = dst;
  for (unsigned int i=0; i<size; i++) {
    (*p8) = (unsigned char)((double)((*p16)-min)/(double)(max-min)*255.0);
    p8++;
    p16++;
  }
}

uint16 PMDImageIO::Convert_(float val, float lower, float upper) {
  float step=(upper-lower)/UINT16_MAX;
  if (val < lower) return 0;
  if (val > upper) return UINT16_MAX;
  return (uint16)((val-lower)/step);
}

float PMDImageIO::Convert_(uint16 val, float lower, float upper) 
{
  float step=(upper-lower)/UINT16_MAX;
  return val*step+lower;
}



uint16 PMDImageIO::DepthConvert_(float depth) {
  return Convert_(depth, DEPTH_MIN, DEPTH_MAX);
}


float PMDImageIO::DepthConvert_(uint16 depth) {
  return Convert_(depth, DEPTH_MIN, DEPTH_MAX);
}


uint16 PMDImageIO::ModCoeffConvert_(float modcoeff) {
  return Convert_(modcoeff, MODCOEFF_MIN, MODCOEFF_MAX);
}

float PMDImageIO::ModCoeffConvert_(uint16 modcoeff) {
  return Convert_(modcoeff, MODCOEFF_MIN, MODCOEFF_MAX);
}


int PMDImageIO::PostprocessData_(uint8* data2d, uint16* dataDepth, 
                                   uint16* dataModCoeff, 
                                   Image<unsigned char> &Img2D, 
                                   Image<float> &ImgDepth, 
                                   Image<float> &ImgModCoeff, 
                                   PMDImageMetaData &MetaData2D, 
                                   PMDImageMetaData &MetaDataDepth) {
  int ret=0, ret2=0, ret3=0;

  if (data2d!=NULL) {
    if (!Img2D.IsEmpty()) Img2D.Release();
    ret = Img2D.InitWithForeignData(MetaData2D.width, MetaData2D.height,
                                    MetaData2D.channels, 
                                    (unsigned char*) data2d);
    if (ret!=0) BIASERR("Error Loading 2D image" << ret);
    
    if (MetaData2D.channels==3) {
      Img2D.SetColorModel(ImageBase::CM_RGB);
    } else {
      Img2D.SetColorModel(ImageBase::CM_Grey);
    }
    
  }

  if (dataDepth!=NULL) {
    float *dataDepthf = new float[MetaDataDepth.width*MetaDataDepth.height];
    float *pDataDepthf = dataDepthf;
    uint16* pDataDepth = dataDepth;
    for (unsigned int i=0; i<MetaDataDepth.width*MetaDataDepth.height; 
         i++, pDataDepthf++, pDataDepth++) {
      *pDataDepthf = DepthConvert_(*pDataDepth);
    }
    if (!ImgDepth.IsEmpty()) ImgDepth.Release();
    ret2 = ImgDepth.InitWithForeignData(MetaDataDepth.width, 
                                        MetaDataDepth.height,
                                        MetaDataDepth.channels, 
                                        dataDepthf);
    delete [] dataDepth;
    if (ret2!=0) BIASERR("Error Loading Depth image" << ret2);
  }
  
  if (dataModCoeff!=NULL) {
    float *dataModCoefff = new float[MetaDataDepth.width*MetaDataDepth.height];
    float *pDataModCoefff = dataModCoefff;
    uint16* pDataModCoeff = dataModCoeff;
    for (unsigned int i=0; i<MetaDataDepth.width*MetaDataDepth.height; 
         i++, pDataModCoefff++, pDataModCoeff++) {
      *pDataModCoefff = ModCoeffConvert_(*pDataModCoeff);
    }
    if (!ImgModCoeff.IsEmpty()) ImgModCoeff.Release();
    ret3 = ImgModCoeff.InitWithForeignData(MetaDataDepth.width, 
                                           MetaDataDepth.height,
                                           MetaDataDepth.channels, 
                                           dataModCoefff);
    delete [] dataModCoeff;
    if (ret3!=0) BIASERR("Error Loading Modulation Coefficients image" 
                         << ret3);
  }
  return ret+ret2+ret3;
}


int PMDImageIO::LoadMetaData_(TIFF* tiffimage, PMDImageMetaData &MetaData) {

  double* cont=NULL;
  int16 count;
  uint16 channels;
  uint32 width, height;

  if(TIFFGetField(tiffimage, TIFFTAG_IMAGEWIDTH, &width) == 0){
    BIASERR("Image does not define its width");
    return -1;
  } else {
    MetaData.width = width;
  }
  
  if(TIFFGetField(tiffimage, TIFFTAG_IMAGELENGTH, &height) == 0){
    BIASERR("Image does not define its height");
    return -1;
  } else {
    MetaData.height = height;
  }
  
  if((TIFFGetField(tiffimage, TIFFTAG_SAMPLESPERPIXEL, &channels) == 0)){
    BIASERR("Wrong number of image planes!");
    return -1;
  } else {
    MetaData.channels =channels;
  }

  if(TIFFGetField(tiffimage, TIFFTAG_PRINCIPALPOINT, &count, &cont) == 0 || 
     (count!=2)){
    BIASERR("Error while Loading Principal point!");
    return -1;
  } else {
    MetaData.PrincipalX = cont[0];
    MetaData.PrincipalY = cont[1];
  }

  if(TIFFGetField(tiffimage, TIFFTAG_FOCALLENGTHX, &count, &cont) == 0 || 
     (count!=1)){
    BIASERR("Error while Loading focal length!");
    return -1;
  } else {
    MetaData.FocallengthX = cont[0];
  }

  if(TIFFGetField(tiffimage, TIFFTAG_ASPECTRATIO, &count, &cont) == 0 || 
     (count!=1)){
    BIASERR("Error while Loading aspectratio!");
    return -1;
  } else {
    MetaData.AspectRatio = cont[0];
  }

  if(TIFFGetField(tiffimage, TIFFTAG_UNDISTORTION, &count, &cont) == 0 || 
     (count!=4)){
    BIASERR("Error while Loading bouget undistortion parameters!");
    return -1;
  } else {
    MetaData.UndistCoeff[0] = cont[0];
    MetaData.UndistCoeff[1] = cont[1];
    MetaData.UndistCoeff[2] = cont[2];
    MetaData.UndistCoeff[3] = cont[3];
  }

  if(TIFFGetField(tiffimage, TIFFTAG_CAMERACENTER, &count, &cont) == 0 || 
     (count!=3)){
    BIASERR("Error while Loading camera center!");
    return -1;
  } else {
    MetaData.CameraCenter[0] = cont[0];
    MetaData.CameraCenter[1] = cont[1];
    MetaData.CameraCenter[2] = cont[2];
  }
  
  if(TIFFGetField(tiffimage, TIFFTAG_CAMERAROTATION, &count, &cont) == 0 || 
     (count!=3)){
    BIASERR("Error while Loading camera rotation!");
    return -1;
  } else {
    MetaData.CameraRotation[0] = cont[0];
    MetaData.CameraRotation[1] = cont[1];
    MetaData.CameraRotation[2] = cont[2];
  }
  
  return 0;
}
 
int PMDImageIO::SaveMetaData_(TIFF* tiffimage, 
                                const PMDImageMetaData &MetaData) {
  
  double princ[] = {MetaData.PrincipalX, MetaData.PrincipalY};
  TIFFSetField(tiffimage, TIFFTAG_PRINCIPALPOINT, 2, &princ);
  double f[] = {MetaData.FocallengthX};
  TIFFSetField(tiffimage, TIFFTAG_FOCALLENGTHX, 1, &f);
  double a[] = {MetaData.AspectRatio};
  TIFFSetField(tiffimage, TIFFTAG_ASPECTRATIO, 1, &a);
  TIFFSetField(tiffimage, TIFFTAG_UNDISTORTION, 4, &MetaData.UndistCoeff);
  TIFFSetField(tiffimage, TIFFTAG_CAMERACENTER, 3, &MetaData.CameraCenter);
  TIFFSetField(tiffimage, TIFFTAG_CAMERAROTATION, 3, 
               &MetaData.CameraRotation);

  return 0;
}


void PMDImageIO::NewTagsInitialize_() 
{
  static int first_time=1;

  if (! first_time) 
    return; /* Been there. Done that. */

  first_time = 0;

  /* Grab the inherited method and install */
  ParentExtender = TIFFSetTagExtender(NewTagsCallback_);
}


void PMDImageIO::NewTagsCallback_(TIFF *tif) {
  /* Install the extended Tag field info */
  TIFFMergeFieldInfo(tif, tiffFieldInfo, 
    sizeof(tiffFieldInfo)/sizeof(tiffFieldInfo[0]));

  /* Since the client module may have overridden
  * the default directory method, we call it now to
  * allow it to set up the rest of its own methods.
  */

  if (ParentExtender) 
    (*ParentExtender)(tif);
}