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

#include "Geometry/ProjectionParametersIO.hh"
#include <string>
#include <Base/Image/ImageAttributes.hh>
#include <MathAlgo/Interpolator.hh>
#include <fstream>
#include <Geometry/RMatrix.hh>
#include <Base/Common/FileHandling.hh>


using namespace BIAS;
using namespace std;


void ProjectionParametersIO::Initialize(CameraData &cd) {

  cd.CamModel_ = "";
  cd.CamID_ = 1;
  cd.LensModel_ = "";
  cd.LensIsSpherical_ = false;
  cd.zoomCamera_ =false;

  cd.width_=cd.height_=0;
  cd.principalX_ = cd.principalY_ =0;
  cd.cellSizeX_ = cd.cellSizeY_ = 1;
  cd.focallengthDef_ = 0;
  cd.kc1Def_ = cd.kc2Def_ = cd.kc3Def_ = cd.kc4Def_ = 0;
  cd.knownparams_vect_.clear();
  cd.IlluCorrX_.clear();
  cd.IlluCorrY_.clear();
  cd.AngleCorrX_.clear();
  cd.AngleCorrY_.clear();
  cd.acCoeff0_ = cd.acCoeff1_ = cd.acCoeff2_ = cd.acCoeff3_ = cd.acCoeff4_ = 0;
  cd.MaxCamAngle_ = 0;
  cd.radius_ = 0;

  cd.PoseInRigCenter_.SetZero();
  cd.PoseInRigYaw_=0;
  cd.PoseInRigPitch_=0;
  cd.PoseInRigRoll_=0;

}
void ProjectionParametersIO::InitializeSensor(SensorData &sd) {

  sd.PoseInRigCenter_.SetZero();
  sd.PoseInRigYaw_=0;
  sd.PoseInRigPitch_=0;
  sd.PoseInRigRoll_=0;

}

int ProjectionParametersIO::ReadFromBBC(const std::string& filename,
                                        CameraData& cd,
                                        const double& addppx,
                                        const double& addppy,
                                        const bool silent) {
  std::ifstream ifs(filename.c_str());
  return ReadFromBBCStream(ifs, cd, addppx, addppy, silent);
}

int ProjectionParametersIO::ReadFromEXIF(const std::string& filename,
                                         CameraData& cd) {

  Initialize(cd);

  ImageAttributes exifdata;
  exifdata.InitExifTags();
  int result = exifdata.Read(filename);
  if (result!=0) return result;


  string make, model;
  exifdata.GetHardwareName(make, model);
  cd.CamModel_ = make + "___"+model;

  cd.CamID_ = 0;
  cd.LensModel_ = "unknown";
  cd.LensIsSpherical_ = false;

  result = exifdata.GetPixelSizeXMeter(cd.cellSizeX_);
  if (result<0) return result;
  result = exifdata.GetPixelSizeYMeter(cd.cellSizeY_);
  if (result<0) return result;

  int width=0, height=0;
  result = exifdata.GetImageDimensions(width, height);
  if (result<0) return result;
  cd.width_ = width;
  cd.height_ = height;

  cd.aspectratio_ = cd.cellSizeX_ / cd.cellSizeY_;

  result = exifdata.GetPrincipalPoint(cd.principalX_, cd.principalY_);
  if (result<0) return result;

  result = exifdata.GetFocalLengthXPixel(cd.focallengthDef_);
  if (result<0) return result;

  cd.zoomCamera_ = false;
  cd.knownparams_vect_.clear();

  return 0;
}


int ProjectionParametersIO::ReadFromBBCStream(ifstream &ifs,
                                              CameraData& cd,
                                              const double& addppx,
                                              const double& addppy,
                                              const bool silent) {
  Vector3<double> BBCC(0.0), BBCA(0.0), BBCUp(0.0);
  unsigned int BBCWidth=0, BBCHeight=0;
  // Pixelsize and FocalLength are in [m] (meter)
  double BBCPxSizeX=0, BBCPxSizeY=0, BBCCxShift=0,
    BBCCyShift=0,BBCFocalLength=0,BBCk3=0,BBCk5=0,BBCCircleRadius=0;
  bool isSpherical = false;
  string key;
  int BBCIdent = -1;
  istringstream iss;
  char linebuf[201];
  while (ifs.getline(linebuf,200)) {
//     cout <<"read line: "<<linebuf<<endl;
    iss.clear();
    iss.str(linebuf);
    iss >> key;
    if (key == "Ident")             iss >> BBCIdent;
    else if (key == "CVec")              iss >> BBCC[0]>> BBCC[1]>> BBCC[2];
    else if (key == "AVec")              iss >> BBCA[0]>> BBCA[1]>> BBCA[2];
    else if (key == "UpVec")             iss >> BBCUp[0]>> BBCUp[1]>> BBCUp[2];
    else if (key == "Width")             iss >> BBCWidth;
    else if (key == "Height")            iss >> BBCHeight;
    else if (key == "PixelSizeX")        iss >> BBCPxSizeX;
    else if (key == "PixelSizeY")        iss >> BBCPxSizeY;
    else if (key == "CenterPointShiftX") iss >> BBCCxShift;
    else if (key == "CenterPointShiftY") iss >> BBCCyShift;
    else if (key == "FocalLength")       iss >> BBCFocalLength;
    else if (key == "RadDistK3")         iss >> BBCk3;
    else if (key == "RadDistK5")         iss >> BBCk5;
    else if (key == "CircleRadius")      iss >> BBCCircleRadius;
    else if (key.find("o_SphericalCamera")!=string::npos) isSpherical = true;
    if (key == "}") break; // end of entry
  }

#ifdef BIAS_DEBUG
  if (true){//!silent){
    if (isSpherical)
      cout<<"BBC parameters are spherical"<<endl;
    else
      cout<<"BBC parameters are perspective"<<endl;
    cout <<"BBC CVec: "<<BBCC<<endl;
    cout <<"BBC AVec: "<<BBCA<<" with norm="<< BBCA.NormL2()<<endl;
    cout <<"BBC UpVec: "<<BBCUp<<" with norm="<< BBCUp.NormL2()<<endl;
    cout <<"BBC Width: "<<BBCWidth<<endl;
    cout <<"BBC Height: "<<BBCHeight<<endl;
    cout <<"BBC PixelSizeX: "<<BBCPxSizeX<<endl;
    cout <<"BBC PixelSizeY: "<<BBCPxSizeY<<endl;
    cout <<"BBC CenterPointShiftX: "<<BBCCxShift<<endl;
    cout <<"BBC CenterPointShiftY: "<<BBCCyShift<<endl;
    cout <<"BBC FocalLength: "<<BBCFocalLength<<endl;
    cout <<"BBC Radials "<<BBCk3<<" "<<BBCk5<<endl;
    cout  <<"CircleRadius"<< BBCCircleRadius<<endl;
  }
#endif

  Matrix3x3<double> K;

  K.SetIdentity();
  K[0][0] = BBCFocalLength / BBCPxSizeX; // fx
  K[1][1] = BBCFocalLength / BBCPxSizeY; // fy

  // compute principle point, addppx and addppy are useful in case
  // the centerpointshift is determined separately
  K[0][2] = double(BBCWidth-1)/2.0  + (BBCCxShift+addppx) / BBCPxSizeX;
  K[1][2] = double(BBCHeight-1)/2.0 + (BBCCyShift+addppy) / BBCPxSizeY;

  Vector3<double> V,C,A,H;
  V = -1.0 * BBCUp;

  C[0] = BBCC[0];
  C[1] = BBCC[1];
  C[2] = BBCC[2];

  A[0] = BBCA[0];
  A[1] = BBCA[1];
  A[2] = BBCA[2];


  H = V.CrossProduct(A);

  RMatrix R;
  for (unsigned int i=0;i<3;i++){
    R[i][0] = H[i];
    R[i][1] = V[i];
    R[i][2] = A[i];
  }
  R.EnforceConstraints();
  Initialize(cd);

  stringstream ss;
  ss<<"FreeD_Camera_ID"<<BBCIdent;
  cd.CamModel_ = ss.str();


  cd.width_ = BBCWidth;
  cd.height_ =  BBCHeight;
  if ( cd.width_==0 || cd.height_==0) {
    if (!silent) {
      BIASERR("Sensor-data incomplete. Camera ID=" << cd.CamID_
              << ", Image Width= " << cd.width_
              << ", Image Height= " << cd.height_);
    }
    return -1;
  }

  // in meter:
  cd.cellSizeX_ = BBCPxSizeX;
  cd.cellSizeY_ = BBCPxSizeY;
  cd.aspectratio_ = cd.cellSizeX_/cd.cellSizeY_;

  cd.principalX_ = K[0][2];
  cd.principalY_ = K[1][2];

  cd.LensIsSpherical_ = isSpherical;

  if (isSpherical) {
    BIASASSERT(BBCPxSizeX == BBCPxSizeY);
    BIASASSERT(K[0][0]>0.0);
    // radius in pixels
    cd.radius_ = floor(BBCCircleRadius / BBCPxSizeX);
    cd.MaxCamAngle_ =  asin(double(cd.radius_) / K[0][0]) / BBCk3;
    cd.AngleCorrX_.push_back(0.0);
    cd.AngleCorrY_.push_back(0.0);
    for (unsigned int r=1; r<=cd.radius_; r++) {
      double idealTheta = cd.MaxCamAngle_ * double(r) / double(cd.radius_);
      double realTheta  = asin(double(r) / K[0][0]) / BBCk3;
      cout<<"ideal theta = "<< idealTheta<<" real="<<realTheta <<endl;

      cd.AngleCorrX_.push_back(idealTheta);
      cd.AngleCorrY_.push_back(realTheta);
    }
    K.SetIdentity();
    cd.focallengthDef_ = 1.0;
    cd.kc1Def_ = 0;
    cd.kc2Def_ = 0;
    cd.kc3Def_ = 0;
    cd.kc4Def_ = 0;
  } else {
    cd.focallengthDef_ = K[0][0];

    // BBC measures radial distortion coefficients in meters at the chip
    // We compute coefficients also in space but on the w=1 plane (or in the
    // image with focallengthx=focallengthy=1pixel in pixels).
    // We have to convert the coefficients k3 = k3bbc * BBCFocalLength^2
    // and similar k5:

    cd.kc1Def_ = -1.0 * BBCk3 * BBCFocalLength * BBCFocalLength;
    cd.kc2Def_ = -1.0 * BBCk5 * BBCFocalLength * BBCFocalLength *
      BBCFocalLength * BBCFocalLength;
    // tangential
    cd.kc3Def_ = 0;
    cd.kc4Def_ = 0;
  }

  cd.zoomCamera_      = false;
  cd.LensModel_ = "UnknownLens";



  cd.PoseInRigCenter_ = C;
  // fill angles in the same order Projection::InitFromCameraData uses them:
  R.GetRotationAnglesXYZ(cd.PoseInRigPitch_,
                         cd.PoseInRigYaw_,
                         cd.PoseInRigRoll_);

  return 0;
}


int ProjectionParametersIO::SetFromKMatrix(const KMatrix& K,
                                           const int w, const int h,
                                           CameraData& cd) {
  Initialize(cd);

  cd.width_    = w;
  cd.height_   = h;
  cd.CamModel_ = "fromkmatrix";

  // you can set any value here but not zero !
  cd.CamID_ = 42;
  cd.LensModel_ = "unknown";
  cd.LensIsSpherical_ = false;

  cd.cellSizeX_  = 1;
  cd.cellSizeY_  = K[0][0] / K[1][1];
  cd.principalX_ = K[0][2];
  cd.principalY_ = K[1][2];

  cd.focallengthDef_ = K[0][0];

  cd.aspectratio_ = cd.cellSizeX_/cd.cellSizeY_;

  cd.knownparams_vect_.clear();
  return 0;

}


#ifdef BIAS_HAVE_XML2

int ProjectionParametersIO::WriteRigData(const std::string &filename,
                                         const std::vector<CameraData>& vcd,
                                         const std::vector<SensorData>& vsd) {

  XMLIO myXML;
  xmlNodePtr rootNode, childNode1, childNode2, childNode3;
  rootNode = myXML.create("rig");
  myXML.addAttribute(rootNode, "version", std::string("1.0") );
  for (unsigned int i=0; i<vcd.size(); i++) {
    childNode1 = myXML.addChildNode(rootNode, "cameraDescription");
    AddCameraDataToRig(myXML, childNode1, vcd[i]);
    childNode2 = myXML.addChildNode(childNode1, "poseInRig");
    myXML.addComment(childNode2, "Pitch, Yaw and Roll are the Rotation_ angles of the camera around the rigs X, Y and Z Axes relative to the rig Rotation_. The Rotation_s are applied in the order XYZ or PYR with fixed axes.");
    childNode3 = myXML.addChildNode(childNode2, "x");
    myXML.addContent(childNode3, vcd[i].PoseInRigCenter_[0]);
    childNode3 = myXML.addChildNode(childNode2, "y");
    myXML.addContent(childNode3, vcd[i].PoseInRigCenter_[1]);
    childNode3 = myXML.addChildNode(childNode2, "z");
    myXML.addContent(childNode3, vcd[i].PoseInRigCenter_[2]);
    myXML.addComment(childNode2,"Rotation_ direction: from rig reference system to camera frame.");
    childNode3 = myXML.addChildNode(childNode2, "yaw");
    myXML.addContent(childNode3, vcd[i].PoseInRigYaw_);
    childNode3 = myXML.addChildNode(childNode2, "pitch");
    myXML.addContent(childNode3, vcd[i].PoseInRigPitch_);
    childNode3 = myXML.addChildNode(childNode2, "roll");
    myXML.addContent(childNode3, vcd[i].PoseInRigRoll_);
  }

  for (unsigned int i=0; i<vsd.size(); i++) {
    childNode1 = myXML.addChildNode(rootNode, "sensorDescription");
    childNode2 = myXML.addChildNode(childNode1, "poseInRig");
    childNode3 = myXML.addChildNode(childNode2, "x");
    myXML.addContent(childNode3, vsd[i].PoseInRigCenter_[0]);
    childNode3 = myXML.addChildNode(childNode2, "y");
    myXML.addContent(childNode3, vsd[i].PoseInRigCenter_[1]);
    childNode3 = myXML.addChildNode(childNode2, "z");
    myXML.addContent(childNode3, vsd[i].PoseInRigCenter_[2]);
    myXML.addComment(childNode2,"Rotation_ direction: from rig reference system to sensor frame.");

    childNode3 = myXML.addChildNode(childNode2, "pitch");
    myXML.addContent(childNode3, vsd[i].PoseInRigPitch_);
    childNode3 = myXML.addChildNode(childNode2, "yaw");
    myXML.addContent(childNode3, vsd[i].PoseInRigYaw_);
    childNode3 = myXML.addChildNode(childNode2, "roll");
    myXML.addContent(childNode3, vsd[i].PoseInRigRoll_);
  }
  myXML.write(filename);
  return 0;
}


int ProjectionParametersIO::ReadRigData(const std::string &filename,
                                        std::vector<CameraData>& vcd,
                                        const bool silent) {
  std::vector<SensorData> vsd;
  int ret = ReadRigData(filename,vcd,vsd);
  if (vsd.size()>0) {
    if (!silent) {
      BIASERR("The data file contained sensor data, use ReadRigData"
              << "with vector<SensorData> to get it.");
    }
  }
  return ret;
}


int ProjectionParametersIO::ReadRigData(const std::string &filename,
                                        std::vector<CameraData>& vcd,
                                        std::vector<SensorData>& vsd,
                                        const bool silent) {
  vcd.clear();
  vsd.clear();

  // TODO: check BIAS_HAVE_XML2 ! JW
  XMLIO myXML;
  xmlNodePtr rootNode, childNode1, childNode2;

  rootNode = myXML.read(filename);
  if (rootNode==NULL) {
    if (!silent) {
      BIASERR("Rig-Parameter-File not found or parse error: " << filename <<" cwd="<< FileHandling::GetCwd() );
    }
    return -1;
  }

  if (myXML.getNodeName(rootNode)!="rig" &&
      myXML.getNodeName(rootNode)!="Rig") {
    if (myXML.getNodeName(rootNode)!="camera" &&
        myXML.getNodeName(rootNode)!="Camera") {
      if (!silent) {
        BIASERR("Root node has to be \"rig\" or \"camera\"");
      }
      return -1;
    } else {
      //Root Node is "camera"
      vcd.resize(1);
      return ReadCameraData(filename, vcd[0]);
    }
  } else {
    // Root node is "Rig"
    double vers=0;
    vers = myXML.getAttributeValueDouble(rootNode, "version");
    if (vers==0) {
      vers = myXML.getAttributeValueDouble(rootNode, "Version");
    }
    if (vers!=RIG_PARAMETER_CURRENT_XML_VERSION) {
      BIASWARN("Rig-Parameter File Version (" << vers << ")is Deprecated, "
              << "current version is " << RIG_PARAMETER_CURRENT_XML_VERSION);
      if (vers==0.8) {
        ReadRigDataV0_8(filename, vcd);
        return 0;
      } else {
        return -1;
      }
    }

    childNode1 = myXML.getFirstChild(rootNode);
    if (childNode1 == NULL) {
      if (!silent) {
        BIASERR("No CameraDescriptions in Rig, empty Rig???");
      }
      return -1;
    }

    int CamCount=0;
    int SensCount=0;
    CameraData cd;
    SensorData sd;
    do {
      if (myXML.getNodeName(childNode1)=="cameraDescription") {
        Initialize(cd);
        childNode2 = myXML.getChild(childNode1, "camera");
        if (childNode2 == NULL) {
          if (!silent) {
            BIASERR("\"CameraDescription\" without a \"Camera\" is not possible.");
          }
          return -1;
        }  else {
          if (ReadCameraData(myXML, childNode2, cd)<0) return -1;
        }
        childNode2 = myXML.getChild(childNode1, "poseInRig");
        if (childNode2 == NULL) {
          if (!silent) {
            BIASERR("\"cameraDescription\" without a \"poseInRig\" is not possible.");
          }
          return -1;
        }  else {
          cd.PoseInRigCenter_[0] =
            myXML.getNodeContentDouble(myXML.getChild(childNode2, "x"));
          cd.PoseInRigCenter_[1] =
            myXML.getNodeContentDouble(myXML.getChild(childNode2, "y"));
          cd.PoseInRigCenter_[2] =
            myXML.getNodeContentDouble(myXML.getChild(childNode2, "z"));
          cd.PoseInRigYaw_ =
            myXML.getNodeContentDouble(myXML.getChild(childNode2,"yaw"));
          cd.PoseInRigPitch_ =
            myXML.getNodeContentDouble(myXML.getChild(childNode2,"pitch"));
          cd.PoseInRigRoll_ =
            myXML.getNodeContentDouble(myXML.getChild(childNode2,"roll"));
        }
        vcd.push_back(cd);
        CamCount++;
      }

      if (myXML.getNodeName(childNode1)=="sensorDescription") {
        InitializeSensor(sd);
        childNode2 = myXML.getChild(childNode1, "poseInRig");
        if (childNode2 == NULL) {
          if (!silent) {
            BIASERR("\"sensorDescription\" without a \"poseInRig\" is not possible.");
          }
          return -1;
        }  else {
          sd.PoseInRigCenter_[0] =
            myXML.getNodeContentDouble(myXML.getChild(childNode2, "x"));
          sd.PoseInRigCenter_[1] =
            myXML.getNodeContentDouble(myXML.getChild(childNode2, "y"));
          sd.PoseInRigCenter_[2] =
            myXML.getNodeContentDouble(myXML.getChild(childNode2, "z"));
          sd.PoseInRigYaw_ =
            myXML.getNodeContentDouble(myXML.getChild(childNode2,"yaw"));
          sd.PoseInRigPitch_ =
            myXML.getNodeContentDouble(myXML.getChild(childNode2,"pitch"));
          sd.PoseInRigRoll_ =
            myXML.getNodeContentDouble(myXML.getChild(childNode2,"roll"));
        }
        vcd.push_back(cd);
        SensCount++;
      }
      childNode1 = myXML.getNextChild(childNode1);
    } while (childNode1!=NULL);
  }

  if (vcd.size()==0) return -1;

  return 0;
}


int ProjectionParametersIO::ReadRigDataV0_8(const std::string &filename,
                                            std::vector<CameraData>& vcd,
                                            const bool silent) {

  XMLIO myXML;
  xmlNodePtr rootNode, childNode1, childNode2;

  rootNode = myXML.read(filename);
  if (rootNode==NULL) {
    if (!silent) {
      BIASERR("Rig-Parameter-File not found or parse error: " << filename);
    }
    return -1;
  }

  // Root node is "Rig"
  childNode1 = myXML.getFirstChild(rootNode);
  if (childNode1 == NULL) {
    if (!silent) {
      BIASERR("No CameraDescriptions in Rig, empty Rig???");
    }
    return -1;
  }

  int CamCount=0;
  CameraData cd;
  do {
    if (myXML.getNodeName(childNode1)=="CameraDescription") {
      Initialize(cd);
      childNode2 = myXML.getChild(childNode1, "Camera");
      if (childNode2 == NULL) {
        if (!silent) {
          BIASERR("\"CameraDescription\" without a \"Camera\" is not possible.");
        }
        return -1;
      }  else {
        if (ReadCameraData(myXML, childNode2, cd)<0) return -1;
      }
      childNode2 = myXML.getChild(childNode1, "PoseInRig");
      if (childNode2 == NULL) {
        if (!silent) {
          BIASERR("\"CameraDescription\" without a \"PoseInRig\" is not possible.");
        }
        return -1;
      }  else {
        cd.PoseInRigCenter_[0] =
          myXML.getAttributeValueDouble(childNode2, "X");
        cd.PoseInRigCenter_[1] =
          myXML.getAttributeValueDouble(childNode2, "Y");
        cd.PoseInRigCenter_[2] =
          myXML.getAttributeValueDouble(childNode2, "Z");
        cd.PoseInRigYaw_ =
          myXML.getAttributeValueDouble(childNode2,"Yaw");
        cd.PoseInRigPitch_ =
          myXML.getAttributeValueDouble(childNode2,"Pitch");
        cd.PoseInRigRoll_ =
          myXML.getAttributeValueDouble(childNode2,"Roll");
      }
      vcd.push_back(cd);
      CamCount++;
    }
    childNode1 = myXML.getNextChild(childNode1);
  } while (childNode1!=NULL);

  if (vcd.size()==0) return -1;

  return 0;
}


int ProjectionParametersIO::WriteCameraData(const std::string& Filename,
                                            const CameraData& cd) {

  XMLIO myXML;
  xmlNodePtr rootNode;
  rootNode = myXML.create("camera");
  myXML.addAttribute(rootNode, "version",
                     CAMERA_PARAMETER_CURRENT_XML_VERSION);
  AddCameraDataToNode(myXML, rootNode, cd);
  myXML.write(Filename);
  return 0;

}


int ProjectionParametersIO::AddCameraDataToRig(XMLIO &myXML,
                                               xmlNodePtr &cameraDataNode,
                                               const CameraData &cd) {

  xmlNodePtr childNode;
  childNode = myXML.addChildNode(cameraDataNode, "camera");
  myXML.addAttribute(childNode, "version",
      std::string("2.0") );
  AddCameraDataToNode(myXML, childNode, cd);
  return 0;

}


int ProjectionParametersIO::AddCameraDataToNode(XMLIO &myXML,
                                                xmlNodePtr &rootNode,
                                                const CameraData &cd) {

  unsigned int i;
  xmlNodePtr  childNode2, childNode3, childNode4;
  myXML.addComment(rootNode, "\"Model\" is an arbitrary camera name.\n \"SerialID\" is i.e. the camera firewire-bus ID.\n \"ImageWidth\" and \"ImageHeight\" are in pixel.\n \"CellSizeX\" is the size of one CCD-cell in meter.\n \"AspectRatio\" is CellsizeX/CellSizeY (same as FocalLengthY/FocalLengthX).");
  childNode2 = myXML.addChildNode(rootNode, "Sensor");
  myXML.addAttribute(childNode2, "Model", cd.CamModel_ );
  myXML.addAttribute(childNode2, "SerialID", cd.CamID_);
  myXML.addAttribute(childNode2, "ImageWidth", (int) cd.width_);
  myXML.addAttribute(childNode2, "ImageHeight", (int) cd.height_);
  myXML.addAttribute(childNode2, "CellSizeX", cd.cellSizeX_);
  myXML.addAttribute(childNode2, "AspectRatio", cd.aspectratio_);
  myXML.addComment(rootNode, "\"Model\" is an arbitrary name.\n \"Type\" can be either \"Spherical\" or \"Perspective\"");
  childNode2 = myXML.addChildNode(rootNode, "Lens");
  myXML.addAttribute(childNode2, "Model", cd.LensModel_ );
  string LensType="";
  if (cd.LensIsSpherical_) {
    LensType="Spherical";
  } else {
    LensType="Perspective";
  }
  myXML.addAttribute(childNode2, "Type", LensType);
  if (cd.LensIsSpherical_) {
    childNode3 = myXML.addChildNode(childNode2, "AngleCalib");
    myXML.addComment(childNode3, "Measurement points of angle calibration curve.\n \"AngRay\" is the angle of the viewing ray, \"AngDeformed\" is the Angle deformed by the camera.\n The maximium \"AngRay\" is the cameras FoV.");
    for (i=0; i<cd.AngleCorrX_.size(); i++) {
      childNode4 = myXML.addChildNode(childNode3, "MeasPt");
      myXML.addAttribute(childNode4, "AngRay", cd.AngleCorrX_[i]);
      myXML.addAttribute(childNode4, "AngDeformed", cd.AngleCorrY_[i]);
    }
    if (cd.acCoeff0_!=0 || cd.acCoeff1_!=0 || cd.acCoeff2_!=0 ||
        cd.acCoeff3_!=0 || cd.acCoeff4_!=0) {
      childNode3 = myXML.addChildNode(childNode2, "AngleCalibPoly");
      myXML.addComment(childNode3, "Polynome coefficients (polynomial degree 4 CN*x^N) from matlab calibration toolbox, see: http://asl.epfl.ch/~scaramuz/research/Davide_Scaramuzza_files/Research/OcamCalib_Tutorial.htm");
      myXML.addAttribute(childNode3, "MaxCamAngle", cd.MaxCamAngle_);
      myXML.addAttribute(childNode3, "C0", cd.acCoeff0_);
      myXML.addAttribute(childNode3, "C1", cd.acCoeff1_);
      myXML.addAttribute(childNode3, "C2", cd.acCoeff2_);
      myXML.addAttribute(childNode3, "C3", cd.acCoeff3_);
      myXML.addAttribute(childNode3, "C4", cd.acCoeff4_);
    }
  } else {
    childNode3 = myXML.addChildNode(childNode2, "PerspectiveCalib");
    myXML.addComment(childNode3, "\"FocalLength\" is FocalLengthX in meters, if you want to give it in pixel set \"CellSizeX\"=1.\n\"k1\" and \"k2\" are radial distortion; \"p1\", \"p2\" are tangential distortion.\n Camera calibration constants coming from the matlab toolbox 'http://www.vision.caltech.edu/bouguetj/calib_doc/'.\n Distortion is calculated as x'=x*(1+k1*r^2+k2*r^4)+2*p1*x*y+p2*(r^2+2*x^2) and y'=y*(1+k1*r^2+k2*r^4)+2*p1*(r^2+2*y^2)+2*p2*x*y with r^2=(x^2+y^2).");
    if (cd.focallengthDef_!=0) {
      childNode4 = myXML.addChildNode(childNode3, "Default");
      //convert to mm
      double fl = cd.focallengthDef_ * cd.cellSizeX_;
      myXML.addAttribute(childNode4, "FocalLength", fl);
      myXML.addAttribute(childNode4, "k1", cd.kc1Def_);
      myXML.addAttribute(childNode4, "k2", cd.kc2Def_);
      myXML.addAttribute(childNode4, "p1", cd.kc3Def_);
      myXML.addAttribute(childNode4, "p2", cd.kc4Def_);
    }
    for (i=0; i<cd.knownparams_vect_.size(); i++) {
      childNode4 = myXML.addChildNode(childNode3, "ZoomStep");
      myXML.addAttribute(childNode4, "Zoom", cd.knownparams_vect_[i].zoom);
      //convert to mm
      double fl = cd.knownparams_vect_[i].focallength * cd.cellSizeX_;
      myXML.addAttribute(childNode4, "FocalLength", fl);
  //       myXML.addComment(childNode2,"Focal length in meter, if you want to use the pixel value set pixelSpacing to 1.");
    myXML.addAttribute(childNode4, "k1", cd.knownparams_vect_[i].kc1);
      myXML.addAttribute(childNode4, "k2", cd.knownparams_vect_[i].kc2);
      myXML.addAttribute(childNode4, "p1", cd.knownparams_vect_[i].kc3);
      myXML.addAttribute(childNode4, "p2", cd.knownparams_vect_[i].kc4);
    }
  }
  childNode3 = myXML.addChildNode(childNode2, "VignetteCalib");
  if (cd.LensIsSpherical_) {
    myXML.addComment(childNode3, "The measurements for vignette calibration relate the angle of the viewing ray to an absorbtion factor for the corresponding image pixels.");
    for (i=0; i<cd.IlluCorrX_.size(); i++) {
      childNode4 = myXML.addChildNode(childNode3, "MeasPt");
      myXML.addAttribute(childNode4, "AngRay", cd.IlluCorrX_[i]);
      myXML.addAttribute(childNode4, "Fac", cd.IlluCorrY_[i]);
    }
  } else {
    myXML.addComment(childNode3, "The measurements for vignette calibration relate the distance of a pixel from the image principal point to an absorbtion factor for the corresponding image pixels.");
    for (i=0; i<cd.IlluCorrX_.size(); i++) {
      childNode4 = myXML.addChildNode(childNode3, "MeasPt");
      myXML.addAttribute(childNode4, "Dist", cd.IlluCorrX_[i]);
      myXML.addAttribute(childNode4, "Fac", cd.IlluCorrY_[i]);
    }
  }

  myXML.addComment(rootNode, "The \"PrincipalPoint\" is given as a fraction of \"ImageWidth\"/\"ImageHeight\" with (0.5,0.5) is the image center.");
  childNode2 = myXML.addChildNode(rootNode, "PrincipalPoint");
  myXML.addAttribute(childNode2, "X", cd.principalX_/cd.width_);
  myXML.addAttribute(childNode2, "Y", cd.principalY_/cd.height_);
  if (cd.LensIsSpherical_) {
    myXML.addComment(childNode2, "The \"SphereRadius\" is given as a fraction of \"ImageWidth\".");
    myXML.addAttribute(childNode2, "SphereRadius", cd.radius_/cd.width_);
  }

  return 0;
}


int ProjectionParametersIO::ReadCameraData(const std::string& Filename,
                                           CameraData& cd,
                                           const bool silent) {

  XMLIO myXML;
  xmlNodePtr rootNode;
  rootNode = myXML.read(Filename);
  if (rootNode==NULL) {
    if (!silent) {
      BIASERR("Camera-Parameter-File not found or parse error: " << Filename);
    }
    return -1;
  }

  if (myXML.getNodeName(rootNode)!="camera" &&
      myXML.getNodeName(rootNode)!="Camera") {
    if (!silent) {
      BIASERR("Root node has to be \"camera\"");
    }
    return -1;
  }
  ReadCameraData (myXML, rootNode, cd);

  return 0;
}


int ProjectionParametersIO::ReadCameraData(XMLIO &myXML,
                                           xmlNodePtr &cameraNode,
                                           CameraData& cd,
                                           const bool silent) {

  Initialize(cd);

  double vers = 0;

  vers = myXML.getAttributeValueDouble(cameraNode, "Version");
  if (vers==0) {
    vers = myXML.getAttributeValueDouble(cameraNode, "version");
  }
  if (vers!=CAMERA_PARAMETER_CURRENT_XML_VERSION) {
    BIASWARN("Camera-Parameter Version (" << vers << ") is Deprecated, "
             << "current version is " << CAMERA_PARAMETER_CURRENT_XML_VERSION);
    if (vers==0.8) {
      ReadCameraDataV0_8(myXML, cameraNode, cd);
      return 0;
    } else {
      if (vers==1.0) {
        ReadCameraDataV1_0(myXML, cameraNode, cd);
        return 0;
      } else {
        return -1;
      }
    }
  }

  xmlNodePtr  childNode2, childNode3, childNode4;

  childNode2 = myXML.getChild(cameraNode, "Sensor");
  if (childNode2!=NULL) {
    cd.CamModel_ = myXML.getAttributeValueString(childNode2, "Model");
    cd.CamID_ = myXML.getAttributeValueInt(childNode2, "SerialID");
    cd.width_ = myXML.getAttributeValueInt(childNode2, "ImageWidth");
    cd.height_ = myXML.getAttributeValueInt(childNode2, "ImageHeight");
    if (cd.CamModel_=="" || cd.CamID_==0 || cd.width_==0 || cd.height_==0) {
      if (!silent) {
      BIASERR("Sensor-data incomplete. Camera Model=" << cd.CamModel_
              << ", Camera Serial=" << cd.CamID_
              << ", Image Width=" << cd.width_
              << ", Image Height" << cd.height_);
      }
      return -1;
    }
    cd.cellSizeX_ = myXML.getAttributeValueDouble(childNode2, "CellSizeX");
    cd.aspectratio_ = myXML.getAttributeValueDouble(childNode2, "AspectRatio");
    if (cd.cellSizeX_==0 || cd.aspectratio_==0) {
      cd.aspectratio_=1;
      cd.cellSizeX_=1;
      cd.cellSizeY_=1;
    } else {
     cd.cellSizeY_ = cd.cellSizeX_ / cd.aspectratio_;
    }
  } else {
    if (!silent) {
      BIASERR("Node \"Sensor\" missing.");
    }
    return -1;
  }

  childNode2 = myXML.getChild(cameraNode, "Lens");
  if (childNode2==NULL) {
    if (!silent) {
      BIASERR("Node \"Lens\" missing.");
    }
    return -1;
  }

  cd.LensModel_ = myXML.getAttributeValueString(childNode2, "Model");
  string LensType = myXML.getAttributeValueString(childNode2, "Type");

  if (LensType!="Spherical" && LensType!="Perspective") {
    if (!silent) {
      BIASERR("Unknown Lens-Type, has to be \"Spherical\" or \"Perspective\".");
    }
    return -1;
  }
  if (LensType=="Spherical") {
    cd.LensIsSpherical_ = true;
    cd.zoomCamera_=false;
    childNode3 = myXML.getChild(childNode2, "AngleCalib");
    if (childNode3!=NULL) {
      childNode4 = myXML.getFirstChild(childNode3);
      if (childNode4 == NULL) {
        if (!silent) {
          BIASERR("No angle calibration measurement points.");
        }
        return -1;
      }  else {
        do {
          if (myXML.getNodeName(childNode4)=="MeasPt") {
            // TODO: Write these into SphereParameters class
            double x,y;
            x = myXML.getAttributeValueDouble(childNode4, "AngRay");
            y = myXML.getAttributeValueDouble(childNode4, "AngDeformed");
            cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
          }
          childNode4 = myXML.getNextChild();
          cout<<".";
        } while (childNode4!=NULL);
      }
    }
    childNode3 = myXML.getChild(childNode2, "AngleCalibPoly");
    if (childNode3==NULL && cd.AngleCorrX_.size()==0) {
      if (!silent) {
        BIASERR("One on the nodes \"AngleCalib\" or \"AngleCalibPoly\"" <<
                "is necessary.");
      }
      return -1;
    } else {
      cd.MaxCamAngle_ =
        myXML.getAttributeValueDouble(childNode3, "MaxCamAngle");
      cd.acCoeff0_ = myXML.getAttributeValueDouble(childNode3, "C0");
      cd.acCoeff1_ = myXML.getAttributeValueDouble(childNode3, "C1");
      cd.acCoeff2_ = myXML.getAttributeValueDouble(childNode3, "C2");
      cd.acCoeff3_ = myXML.getAttributeValueDouble(childNode3, "C3");
      cd.acCoeff4_ = myXML.getAttributeValueDouble(childNode3, "C4");
      if (cd.AngleCorrX_.size()==0) {
        InitAngleCorrFromPoly_(cd);
      }
    }
  }
  if (LensType=="Perspective") {
    cd.LensIsSpherical_ = false;
    cd.zoomCamera_=false;
    childNode3 = myXML.getChild(childNode2, "PerspectiveCalib");
    if (childNode3 != NULL) {
      childNode4 = myXML.getFirstChild(childNode3);
      if (childNode4 != NULL) {
        do {
          if (myXML.getNodeName(childNode4)=="Default") {
            cd.focallengthDef_ =
              myXML.getAttributeValueDouble(childNode4, "FocalLength");
            //convert to Pixel
            if (cd.cellSizeX_!=1) {
              cd.focallengthDef_ = cd.focallengthDef_ / cd.cellSizeX_;
            }
            cd.kc1Def_ = myXML.getAttributeValueDouble(childNode4, "k1");
            cd.kc2Def_ = myXML.getAttributeValueDouble(childNode4, "k2");
            cd.kc3Def_ = myXML.getAttributeValueDouble(childNode4, "p1");
            cd.kc4Def_ = myXML.getAttributeValueDouble(childNode4, "p2");
          }
          if (myXML.getNodeName(childNode4)=="ZoomStep") {
            CPDiscreteParam dp;
            dp.zoom=myXML.getAttributeValueDouble(childNode4, "Zoom");
            dp.focallength=myXML.getAttributeValueDouble(childNode4,
                                                         "FocalLength");
             //convert to Pixel
            if (cd.cellSizeX_!=1) {
              dp.focallength = dp.focallength / cd.cellSizeX_;
            }
            dp.kc1=myXML.getAttributeValueDouble(childNode4, "k1");
            dp.kc2=myXML.getAttributeValueDouble(childNode4, "k2");
            dp.kc3=myXML.getAttributeValueDouble(childNode4, "p1");
            dp.kc4=myXML.getAttributeValueDouble(childNode4, "p2");
            cd.knownparams_vect_.push_back(dp);
            cd.zoomCamera_=true;
          }
          childNode4 = myXML.getNextChild();
        } while (childNode4!=NULL);
        if (cd.knownparams_vect_.size()>0 &&
            cd.focallengthDef_==0) {
            cd.focallengthDef_ = cd.knownparams_vect_[0].focallength;
            cd.kc1Def_ = cd.knownparams_vect_[0].kc1;
            cd.kc2Def_ = cd.knownparams_vect_[0].kc2;
            cd.kc3Def_ = cd.knownparams_vect_[0].kc3;
            cd.kc4Def_ = cd.knownparams_vect_[0].kc4;
        }
      }
    }
  }

  childNode3 = myXML.getChild(childNode2, "VignetteCalib");
  if (childNode3 != NULL) {
    childNode4 = myXML.getFirstChild(childNode3);
    if (childNode4 != NULL) {
      do {
        if (myXML.getNodeName(childNode4)=="MeasPt") {
          // TODO: Write these into SphereParameters & cameradata class
          double x,y;
          if (cd.LensIsSpherical_) {
            x = myXML.getAttributeValueDouble(childNode4, "AngRay");
          } else {
            x = myXML.getAttributeValueDouble(childNode4, "Dist");
          }
          y = myXML.getAttributeValueDouble(childNode4, "Fac");
          cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
        }
        childNode4 = myXML.getNextChild();
      } while (childNode4!=NULL);
    }
  }

  childNode2 = myXML.getChild(cameraNode, "PrincipalPoint");
  if (childNode2 != NULL) {
    cd.principalX_ = myXML.getAttributeValueDouble(childNode2, "X");
    cd.principalX_ = cd.width_ * cd.principalX_;
    cd.principalY_ = myXML.getAttributeValueDouble(childNode2, "Y");
    cd.principalY_ = cd.height_ * cd.principalY_;
    if (cd.LensIsSpherical_) {
      if (cd.radius_ > 0) {
        // it would be more intuitive to pass the radius than the maxcamangle
        // this would require to change some of the read logic
        // maybe an improvement for version 3.0 ;-)

        if  (myXML.getAttributeByName(childNode2,"SphereRadius")!=NULL) {
          BIASWARN(endl<<endl<<endl<<"The SphereRadius field in your camera"
                   <<" parameter file is redundant and must be consistent with"
                   <<" the maximum cam angle. Your value of SphereRadius "
                   <<" will be ignored and computed"<<endl<<endl<<endl);
        } else {
          // fine: user erased SphereRadius entry vom camera paramter file
        }
      } else {
        cd.radius_ = myXML.getAttributeValueDouble(childNode2, "SphereRadius");
        cd.radius_ = cd.width_ * cd.radius_;
      }
    }
  } else {
    cd.principalX_ = cd.width_/2.0;
    cd.principalY_ = cd.height_/2.0;
    if (cd.LensIsSpherical_) {
      cd.radius_ = cd.height_/2.0;
    }
  }

  return 0;

}


int ProjectionParametersIO::ReadCameraDataV1_0(XMLIO &myXML,
                                               xmlNodePtr &cameraNode,
                                               CameraData& cd,
                                               const bool silent) {

  Initialize(cd);

  cd.CamModel_ = myXML.getAttributeValueString(cameraNode, "name");

  xmlNodePtr  childNode2, childNode3, childNode4, childNode5, childNode6;

  childNode2 = myXML.getChild(cameraNode, "id");
  cd.CamID_ = myXML.getNodeContentInt(childNode2);
  if ( cd.CamID_==0 ) {
    if (!silent) {
      BIASERR("Sensor-data incomplete. Camera ID must not be 0:" << cd.CamID_);
    }
    return -1;
  }

  childNode2 = myXML.getChild(cameraNode, "sensor");
  if (childNode2!=NULL) {
    childNode3 = myXML.getChild(childNode2, "numberOfPixels");
    childNode4 = myXML.getChild(childNode3, "x");
    cd.width_ = myXML.getNodeContentInt(childNode4);
    childNode4 = myXML.getChild(childNode3, "y");
    cd.height_ = myXML.getNodeContentInt(childNode4);
    if ( cd.width_==0 || cd.height_==0) {
      if (!silent) {
        BIASERR("Sensor-data incomplete. Camera ID=" << cd.CamID_
                << ", Image Width= " << cd.width_
                << ", Image Height= " << cd.height_);
      }
      return -1;
    }

    childNode3 = myXML.getChild(childNode2, "pixelSpacing");
    childNode4 = myXML.getChild(childNode3, "x");
    cd.cellSizeX_ = myXML.getNodeContentDouble(childNode4);
    childNode4 = myXML.getChild(childNode3, "y");
    cd.cellSizeY_ = myXML.getNodeContentDouble(childNode4);
    if (cd.cellSizeX_==0 || cd.cellSizeY_==0) {
      cd.cellSizeX_=1;
      cd.cellSizeY_=1;
    }
    cd.aspectratio_ = cd.cellSizeX_/cd.cellSizeY_;
    childNode3 = myXML.getChild(childNode2, "principalPoint");
    if (childNode3 != NULL) {
      childNode4 = myXML.getChild(childNode3, "x");
      cd.principalX_ = myXML.getNodeContentDouble(childNode4);
      childNode4 = myXML.getChild(childNode3, "y");
      cd.principalY_ = myXML.getNodeContentDouble(childNode4);
      //convert to Pixel
      cd.principalX_ = cd.width_/2.0  + cd.principalX_/cd.cellSizeX_;
      cd.principalY_ = cd.height_/2.0 + cd.principalY_/cd.cellSizeY_;
    } else {
      cd.principalX_ = cd.width_/2.0;
      cd.principalY_ = cd.height_/2.0;
    }

  } else {
    if (!silent) {
      BIASERR("Node \"sensor\" missing.");
    }
    return -1;
  }

  childNode2 = myXML.getChild(cameraNode, "lens");
  if (childNode2!=NULL) {
    cd.LensIsSpherical_=false;
    cd.zoomCamera_=false;
  } else {
    childNode2 = myXML.getChild(cameraNode, "zoomLens");
    if (childNode2!=NULL) {
      cd.LensIsSpherical_=false;
      cd.zoomCamera_=true;
    } else {
      childNode2 = myXML.getChild(cameraNode, "sphericalLens");
      if (childNode2!=NULL) {
        cd.LensIsSpherical_=true;
        cd.zoomCamera_=false;
      } else {
        if (!silent) {
          BIASERR("Camera has no lens-, zoomLens- or sphericalLens-Tag");
        }
        return -1;
      }
    }
  }

  cd.LensModel_ = myXML.getAttributeValueString(childNode2, "name");

  if (cd.LensIsSpherical_) {

    childNode3 = myXML.getChild(childNode2, "angleCalibration");
    if (childNode3==NULL) {
      if (!silent) {
        BIASERR("Node \"angleCalibration\" missing.");
      }
      return -1;
    }
    childNode4 = myXML.getFirstChild(childNode3);
    if (childNode4 == NULL) {
      if (!silent) {
        BIASERR("No angle calibration measurement points.");
      }
      return -1;
    }  else {
      do {
        if (myXML.getNodeName(childNode4)=="measPoint") {
          double x,y;
          childNode5 = myXML.getChild(childNode4, "angleRay");
          x = myXML.getNodeContentDouble(childNode5);
          childNode5 = myXML.getChild(childNode4, "angleDeformed");
          y = myXML.getNodeContentDouble(childNode5);
          cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
        }
        childNode4 = myXML.getNextChild(childNode4);
      } while (childNode4!=NULL);
    }

    childNode3 = myXML.getChild(childNode2, "vignetteCalibration");
    if (childNode3 != NULL) {
      childNode4 = myXML.getFirstChild(childNode3);
      if (childNode4 != NULL) {
        do {
          if (myXML.getNodeName(childNode4)=="measPoint") {
            double x,y;
            childNode5 = myXML.getChild(childNode4, "angleRay");
            x = myXML.getNodeContentDouble(childNode5);
            childNode5 = myXML.getChild(childNode4, "factor");
            y = myXML.getNodeContentDouble(childNode5);
            cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
          }
          childNode4 = myXML.getNextChild(childNode4);
        } while (childNode4!=NULL);
      }
    }

    childNode3 = myXML.getChild(childNode2, "sphereRadius");
    cd.radius_ = myXML.getNodeContentDouble(childNode3);
    if (childNode3==NULL) {
      if (!silent) {
        BIASERR("Sphere radius missing!");
      }
      return -1;
    }

  } else {  //  if (!LensIsSpherical_)
    if (!cd.zoomCamera_) {
      childNode3 = myXML.getChild(childNode2, "focalLength");
      cd.focallengthDef_ = myXML.getNodeContentDouble(childNode3);
      //convert to Pixel
      cd.focallengthDef_ = cd.focallengthDef_/cd.cellSizeX_;
      childNode3 = myXML.getChild(childNode2, "distortion");
      childNode4 = myXML.getChild(childNode3, "radial");
      childNode5 = myXML.getChild(childNode4, "k1");
      cd.kc1Def_ = myXML.getNodeContentDouble(childNode5);
      childNode5 = myXML.getChild(childNode4, "k2");
      cd.kc2Def_ = myXML.getNodeContentDouble(childNode5);
      childNode4 = myXML.getChild(childNode3, "tangential");
      childNode5 = myXML.getChild(childNode4, "p1");
      cd.kc3Def_ = myXML.getNodeContentDouble(childNode5);
      childNode5 = myXML.getChild(childNode4, "p2");
      cd.kc4Def_ = myXML.getNodeContentDouble(childNode5);
    } else { // if (zoomCamera)
      childNode3 = myXML.getFirstChild(childNode2);
      if (childNode3 != NULL) {
        do {
          if (myXML.getNodeName(childNode3)=="zoomStep") {
            CPDiscreteParam dp;
            childNode4 = myXML.getChild(childNode3, "zoom");
            dp.zoom = myXML.getNodeContentDouble(childNode4);
            childNode4=myXML.getChild(childNode3, "focalLength");
            dp.focallength = myXML.getNodeContentDouble(childNode4);
             //convert to Pixel
            dp.focallength = dp.focallength/cd.cellSizeX_;
            childNode4=myXML.getChild(childNode3, "distortion");
            childNode5=myXML.getChild(childNode4, "radial");
            childNode6=myXML.getChild(childNode5, "k1");
            dp.kc1 = myXML.getNodeContentDouble(childNode6);
            childNode6=myXML.getChild(childNode5, "k2");
            dp.kc2 = myXML.getNodeContentDouble(childNode6);
            childNode5=myXML.getChild(childNode4, "tangential");
            childNode6=myXML.getChild(childNode5, "p1");
            dp.kc3 = myXML.getNodeContentDouble(childNode6);
            childNode6=myXML.getChild(childNode5, "p2");
            dp.kc4 = myXML.getNodeContentDouble(childNode6);
            cd.knownparams_vect_.push_back(dp);
          }
          if (myXML.getNodeName(childNode3)=="defaultZoom") {
            if (!silent) {
              BIASERR("defaultZoom is deprecated and has no effect, use different param-file version");
            }
          }
          childNode3 = myXML.getNextChild(childNode3);
        } while (childNode3!=NULL);
        if (cd.knownparams_vect_.size()>0) {
          cd.focallengthDef_ = cd.knownparams_vect_[0].focallength;
          cd.kc1Def_ = cd.knownparams_vect_[0].kc1;
          cd.kc2Def_ = cd.knownparams_vect_[0].kc2;
          cd.kc3Def_ = cd.knownparams_vect_[0].kc3;
          cd.kc4Def_ = cd.knownparams_vect_[0].kc4;
        }
      }
    }
  }

  return 0;
}


int ProjectionParametersIO::ReadCameraDataV0_8(XMLIO &myXML,
                                               xmlNodePtr &cameraNode,
                                               CameraData& cd,
                                               const bool silent) {

  Initialize(cd);

  xmlNodePtr  childNode2, childNode3, childNode4;

  childNode2 = myXML.getChild(cameraNode, "Sensor");
  if (childNode2!=NULL) {
    cd.CamModel_ = myXML.getAttributeValueString(childNode2, "Model");
    cd.CamID_ = myXML.getAttributeValueInt(childNode2, "SerialID");
    cd.width_ = myXML.getAttributeValueInt(childNode2, "ImageWidth");
    cd.height_ = myXML.getAttributeValueInt(childNode2, "ImageHeight");
    if (cd.CamModel_=="" || cd.CamID_==0 || cd.width_==0 || cd.height_==0) {
      if (!silent) {
      BIASERR("Sensor-data incomplete. Camera Model=" << cd.CamModel_
              << ", Camera Serial=" << cd.CamID_
              << ", Image Width=" << cd.width_
              << ", Image Height" << cd.height_);
      }
      return -1;
    }
    cd.cellSizeX_ = myXML.getAttributeValueDouble(childNode2, "CellSizeInMicrX");
    cd.cellSizeY_ = myXML.getAttributeValueDouble(childNode2, "CellSizeInMicrY");
    if (cd.cellSizeX_>100 ||cd.cellSizeY_>100) {
      cd.cellSizeX_ /= 1000.0;
      cd.cellSizeY_ /= 1000.0;
    } else {
      cd.cellSizeX_ /= 1000000.0;
      cd.cellSizeY_ /= 1000000.0;
    }
    if (cd.cellSizeX_==0 || cd.cellSizeY_==0) {
      cd.aspectratio_=1;
      cd.cellSizeX_=1;
      cd.cellSizeY_=1;
    } else {
      cd.aspectratio_ = cd.cellSizeX_/cd.cellSizeY_;
    }
  } else {
    if (!silent) {
      BIASERR("Node \"Sensor\" missing.");
    }
    return -1;
  }

  childNode2 = myXML.getChild(cameraNode, "Lens");
  if (childNode2==NULL) {
    if (!silent) {
      BIASERR("Node \"Lens\" missing.");
    }
    return -1;
  }

  cd.LensModel_ = myXML.getAttributeValueString(childNode2, "Model");
  string LensType = myXML.getAttributeValueString(childNode2, "Type");

  if (LensType!="Spherical" && LensType!="Perspective") {
    if (!silent) {
      BIASERR("Unknown Lens-Type, has to be \"Spherical\" or \"Perspective\".");
    }
    return -1;
  }
  if (LensType=="Spherical") {
    cd.LensIsSpherical_ = true;
    cd.zoomCamera_=false;
    childNode3 = myXML.getChild(childNode2, "AngleCalib");
    if (childNode3==NULL) {
      if (!silent) {
        BIASERR("Node \"AngleCalib\" missing.");
      }
      return -1;
    }
    childNode4 = myXML.getFirstChild(childNode3);
    if (childNode4 == NULL) {
      if (!silent) {
        BIASERR("No angle calibration measurement points.");
      }
      return -1;
    }  else {
      do {
        if (myXML.getNodeName(childNode4)=="MeasPt") {
          // TODO: Write these into SphereParameters class
          double x,y;
          x = myXML.getAttributeValueDouble(childNode4, "AngRay");
          y = myXML.getAttributeValueDouble(childNode4, "AngDeformed");
          cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
        }
        childNode4 = myXML.getNextChild();
      } while (childNode4!=NULL);
    }
  }
  if (LensType=="Perspective") {
    cd.LensIsSpherical_ = false;
    cd.zoomCamera_=false;
    childNode3 = myXML.getChild(childNode2, "PerspectiveCalib");
    if (childNode3 != NULL) {
      childNode4 = myXML.getFirstChild(childNode3);
      if (childNode4 != NULL) {
        do {
          if (myXML.getNodeName(childNode4)=="Default") {
            cd.focallengthDef_ =
              myXML.getAttributeValueDouble(childNode4, "FocalLengthInMM");
            //convert to Pixel
            if (cd.cellSizeX_!=1) {
              cd.focallengthDef_ = cd.focallengthDef_/1000.0/cd.cellSizeX_;
            }
            cd.kc1Def_ = myXML.getAttributeValueDouble(childNode4, "KC1");
            cd.kc2Def_ = myXML.getAttributeValueDouble(childNode4, "KC2");
            cd.kc3Def_ = myXML.getAttributeValueDouble(childNode4, "KC3");
            cd.kc4Def_ = myXML.getAttributeValueDouble(childNode4, "KC4");
          }
          if (myXML.getNodeName(childNode4)=="ZoomStep") {
            CPDiscreteParam dp;
            dp.zoom=myXML.getAttributeValueDouble(childNode4, "Zoom");
            dp.focallength=myXML.getAttributeValueDouble(childNode4,
                                                         "FocalLengthInMM");
             //convert to Pixel
            if (cd.cellSizeX_!=1) {
              dp.focallength = dp.focallength/1000.0 / cd.cellSizeX_;
            }
            dp.kc1=myXML.getAttributeValueDouble(childNode4, "KC1");
            dp.kc2=myXML.getAttributeValueDouble(childNode4, "KC2");
            dp.kc3=myXML.getAttributeValueDouble(childNode4, "KC3");
            dp.kc4=myXML.getAttributeValueDouble(childNode4, "KC4");
            cd.knownparams_vect_.push_back(dp);
            cd.zoomCamera_=true;
          }
          childNode4 = myXML.getNextChild();
        } while (childNode4!=NULL);
        if (cd.knownparams_vect_.size()>0 &&
            cd.focallengthDef_==0) {
            cd.focallengthDef_ = cd.knownparams_vect_[0].focallength;
            cd.kc1Def_ = cd.knownparams_vect_[0].kc1;
            cd.kc2Def_ = cd.knownparams_vect_[0].kc2;
            cd.kc3Def_ = cd.knownparams_vect_[0].kc3;
            cd.kc4Def_ = cd.knownparams_vect_[0].kc4;
        }
      }
    }
  }

  childNode3 = myXML.getChild(childNode2, "VignetteCalib");
  if (childNode3 != NULL) {
    childNode4 = myXML.getFirstChild(childNode3);
    if (childNode4 != NULL) {
      do {
        if (myXML.getNodeName(childNode4)=="MeasPt") {
          // TODO: Write these into SphereParameters & cameradata class
          double x,y;
          if (cd.LensIsSpherical_) {
            x = myXML.getAttributeValueDouble(childNode4, "AngRay");
          } else {
            x = myXML.getAttributeValueDouble(childNode4, "Dist");
          }
          y = myXML.getAttributeValueDouble(childNode4, "Fac");
          cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
        }
        childNode4 = myXML.getNextChild();
      } while (childNode4!=NULL);
    }
  }

  childNode2 = myXML.getChild(cameraNode, "PrincipalPoint");
  if (childNode2 != NULL) {
    cd.principalX_ = myXML.getAttributeValueDouble(childNode2, "X");
    cd.principalY_ = myXML.getAttributeValueDouble(childNode2, "Y");
    if (cd.LensIsSpherical_) {
      cd.radius_ = myXML.getAttributeValueDouble(childNode2, "SphereRadius");
    }
  } else {
    cd.principalX_ = cd.width_/2.0;
    cd.principalY_ = cd.height_/2.0;
    if (cd.LensIsSpherical_) {
      cd.radius_ = cd.height_/2.0;
    }
  }

  return 0;
}
#endif


int ProjectionParametersIO::InitAngleCorrFromPoly_(CameraData &cd)
{
  double pix_def, ang_def, ang_pix_der, ang_opt;
  int steps=14;

  // calculate distortion function
  vector<double> vx, vy;
  double maxradius = 0;
  for (unsigned int x=0; x<cd.width_/2; x++) {
    //ang_pix = atan2(0:floor(nx/2),-polyval([ss(end:-1:1)],[0:floor(nx/2)]));
    vx.push_back(x);
    double pol = cd.acCoeff0_ + (cd.acCoeff1_ + (cd.acCoeff2_  +
      (cd.acCoeff3_ + cd.acCoeff4_ * x)*x)*x)*x;
    double y = atan2(x,-pol);
    vy.push_back(y);
    if (x>maxradius && y<cd.MaxCamAngle_) maxradius = x;
  }
  if (cd.radius_<=0.0) {
    cout<<"maxradius determined as "<<maxradius<<endl;
    cd.radius_ = maxradius;
  }
  // numerically deriving distortion function in 0
  ang_pix_der = vy[1]-vy[0];
  Interpolator ip_inv;
  double dum;
  ip_inv.SetKnotPoints(vy);
  ip_inv.SetControlPoints(vx);

  ip_inv.InitSpline();
  ip_inv.Spline(dum, 0);

  cd.AngleCorrX_.clear();
  cd.AngleCorrY_.clear();

  for (double i=0; i-1e-10<=cd.MaxCamAngle_; i+=cd.MaxCamAngle_/steps) {
    ang_opt = i;

    ip_inv.Spline(pix_def,ang_opt);
    ang_def = pix_def*ang_pix_der;
    cd.AngleCorrX_.push_back(ang_opt);
    cd.AngleCorrY_.push_back(ang_def);
  }
  return 0;
}


void ProjectionParametersIO::SetData_EVID31_147094(CameraData& cd) {

  Initialize(cd);

  CPDiscreteParam dp;
  cd.CamModel_ = "SONY EVI-D31";
  cd.CamID_ = 147094;
  cd.LensModel_ = "LENS_SONY_EVID31_BUILTIN";
  cd.LensIsSpherical_ = false;

  cd.cellSizeX_=1;
  cd.cellSizeY_=1;

  cd.width_=768;
  cd.height_=576;

  cd.aspectratio_=1;

  cd.principalX_=384;
  cd.principalY_=288;

//   exposureTime_ = 0.08;
//   iris_ = 1;
//   reciprocalFocus_ = 1.234;

  cd.knownparams_vect_.clear();

  // must be in descending order regarding the zoom!

  dp.zoom=47;
  dp.focallength=910.781402852481733;
  dp.kc1=-0.268280770809586;
  dp.kc2=0.487451704517498;
  dp.kc3=-0.000762042258562;
  dp.kc4=-0.001130194838780;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=45;
  dp.focallength=942.276371802197332;
  dp.kc1=-0.222325918868402;
  dp.kc2=0.303803084817878;
  dp.kc3=-0.000220828107747;
  dp.kc4=-0.002379174938453;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=43;
  dp.focallength=989.223042668483913;
  dp.kc1=-0.230499856719837;
  dp.kc2=0.632569092436950;
  dp.kc3=0.000645301367934;
  dp.kc4=-0.002245120579513;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=41;
  dp.focallength=1046.743156411611380;
  dp.kc1=-0.175147739150835;
  dp.kc2=0.399855896767651;
  dp.kc3=0.001819727735452;
  dp.kc4=-0.004642715440390;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=39;
  dp.focallength=1113.877613258150632;
  dp.kc1=-0.199908717507440;
  dp.kc2=0.735328948045900;
  dp.kc3=0.001857139856879;
  dp.kc4=-0.001726332328807;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=37;
  dp.focallength=1195.129672009601336;
  dp.kc1=-0.194029932886528;
  dp.kc2=1.052137931539009;
  dp.kc3=0.002556010034664;
  dp.kc4=-0.002664280436511;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=35;
  dp.focallength=1284.915674699713918;
  dp.kc1=-0.222381373429422;
  dp.kc2=1.627430874083972;
  dp.kc3=0.001072744905158;
  dp.kc4=-0.001628105221342;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=33;
  dp.focallength=1381.682287410254276;
  dp.kc1=-0.221923278642680;
  dp.kc2=1.734685282276320;
  dp.kc3=0.002486050695331;
  dp.kc4=-0.001291711925732;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=31;
  dp.focallength=1493.437763776483052;
  dp.kc1=-0.224447467694984;
  dp.kc2=1.863840161034580;
  dp.kc3=0.003709677616807;
  dp.kc4=-0.001150445035362;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=29;
  dp.focallength=1616.993704040472039;
  dp.kc1=-0.202923793671622;
  dp.kc2=1.473981778670278;
  dp.kc3=0.003684677768293;
  dp.kc4=-0.002550288087126;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=27;
  dp.focallength=1757.081611652665060;
  dp.kc1=-0.217240281709651;
  dp.kc2=2.140024501420107;
  dp.kc3=0.001114854630183;
  dp.kc4=-0.001295578479362;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=25;
  dp.focallength=1902.05452;
  dp.kc1=-0.23863;
  dp.kc2=1.61900;
  dp.kc3=0.00195;
  dp.kc4=-0.00185;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=23;
  dp.focallength=2074.378333106370974;
  dp.kc1=-0.255567875389573;
  dp.kc2=1.732924024277849;
  dp.kc3=0.001245946585972;
  dp.kc4=-0.000594821663798;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=21;
  dp.focallength=2282.63949;
  dp.kc1=-0.26368;
  dp.kc2=0.44519;
  dp.kc3=0.00174;
  dp.kc4=-0.00076;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=19;
  dp.focallength=2519.628084135987592;
  dp.kc1=-0.332071279316112;
  dp.kc2=0.944781789471998;
  dp.kc3=0.000746973436601;
  dp.kc4=-0.001772703465946;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=17;
  dp.focallength=2824.507670379169667;
  dp.kc1=-0.404343435901478;
  dp.kc2=0.453162060457514;
  dp.kc3=-0.000861720421368;
  dp.kc4=-0.001149130726447;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=15;
  dp.focallength=3226.589266051306367;
  dp.kc1=-0.594320179301222;
  dp.kc2=12.959736505367889;
  dp.kc3=-0.001868658432064;
  dp.kc4=-0.000676806079268;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=13;
  dp.focallength=3788.529115119004928;
  dp.kc1=-0.823072669196453;
  dp.kc2=27.510513372014003;
  dp.kc3=-0.006928032867432;
  dp.kc4=0.002134868203333;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=11;
  dp.focallength=4592.039263971351829;
  dp.kc1=-1.121702159939672;
  dp.kc2=36.994236469912934;
  dp.kc3=-0.014620822228192;
  dp.kc4=0.004860766468908;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=9;
  dp.focallength=5834.063334730769384;
  dp.kc1=-1.473018481165970;
  dp.kc2=41.598117903225528;
  dp.kc3=-0.028692464501407;
  dp.kc4=0.007739406251353;
  cd.knownparams_vect_.push_back(dp);

  dp.zoom=7;
  dp.focallength=5877.390940425404551;
  dp.kc1=-1.786184809649660;
  dp.kc2=109.639598242766894;
  dp.kc3=-0.011555140535335;
  dp.kc4=-0.002211119391028;
  cd.knownparams_vect_.push_back(dp);

  // this is a safety-buffer
  dp.zoom=1000;
  cd.knownparams_vect_.push_back(dp);

}


void ProjectionParametersIO::SetData_OMNITECH_ORIFL_190_3(CameraData& cd) {

  Initialize(cd);

  cd.AngleCorrX_.clear();
  cd.AngleCorrY_.clear();
  cd.IlluCorrX_.clear();
  cd.IlluCorrY_.clear();

  cd.CamModel_ = "Fisheye Camera";
  cd.CamID_ = 0440;
  cd.LensModel_ = "OMNITECH_ORIFL_190_3";
  cd.LensIsSpherical_ = true;

  cd.width_=640;
  cd.height_=480;

  cd.principalX_ = 364;
  cd.principalY_ = 245;

  cd.aspectratio_=1;

  cd.knownparams_vect_.clear();

  cd.cellSizeX_=1;
  cd.cellSizeY_=1;

  double x,y;
  x=0.000; y=1.000*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
  x=0.138; y=1.000*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
  x=0.424; y=0.993*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
  x=0.757; y=0.974*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
  x=1.045; y=0.941*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
  x=1.342; y=0.899*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
  x=1.436; y=0.881*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);
  x=1.652; y=0.828*x;
  cd.AngleCorrX_.push_back(x); cd.AngleCorrY_.push_back(y);

  // Vignette correction x=angle, y=darkening factor
  x=0.005; y=1.000;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.092; y=1.000;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.179; y=1.000;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.266; y=1.003;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.354; y=1.003;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.439; y=1.003;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.527; y=1.003;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.615; y=1.005;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.701; y=1.005;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.788; y=1.005;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.875; y=1.005;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=0.962; y=1.003;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.049; y=0.998;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.135; y=0.993;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.223; y=0.983;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.310; y=0.966;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.398; y=0.941;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.485; y=0.897;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.572; y=0.828;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);
  x=1.660; y=0.716;
  cd.IlluCorrX_.push_back(x); cd.IlluCorrY_.push_back(y);

  // TODO: Implement Max Angle
  //double MaxCamAngle = 1.6581; // 95 deg

  cd.radius_ = 226;
}


void ProjectionParametersIO::SetData_DummyRig(std::vector<CameraData>& vcd) {

  CameraData cd;
  Initialize(cd);
  SetData_EVID31_147094(cd);
  cd.PoseInRigYaw_=0;
  cd.PoseInRigPitch_=0;
  cd.PoseInRigRoll_=0;
  cd.PoseInRigCenter_.SetZero();
  vcd.push_back(cd);

  Initialize(cd);
  SetData_OMNITECH_ORIFL_190_3(cd);
  cd.PoseInRigYaw_=.1;
  cd.PoseInRigPitch_=.2;
  cd.PoseInRigRoll_=-.15;
  cd.PoseInRigCenter_[0]=123.45;
  cd.PoseInRigCenter_[1]=321.54;
  cd.PoseInRigCenter_[2]=666.67;
  vcd.push_back(cd);

}


std::ostream& operator<<(std::ostream& os,
                         std::vector<BIAS::CameraData>& vcd) {

  os << "vector<CameraData>: " << endl;
  for (unsigned int i=0; i<vcd.size(); i++) {
    os << " ----------- Camera " << i << "-----------" << endl;
    os << vcd[i];
    os << endl;
  }

  return os;
}


std::ostream& operator<<(std::ostream& os,
                         std::vector<BIAS::SensorData>& vsd) {

  os << "vector<SensorData>: " << endl;
  for (unsigned int i=0; i<vsd.size(); i++) {
    os << " ----------- Sensor " << i << "-----------" << endl;
    os << vsd[i];
    os << endl;
  }

  return os;
}


std::ostream& operator<<(std::ostream& os,
                         BIAS::CameraData& cd) {

  os << "CamModel_ = " << cd.CamModel_ << endl;
  os << "CamID_ = " << cd.CamID_ << endl;
  os << "LensModel_ = " << cd.LensModel_ << endl;
  os << "LensIsSpherical_ = " << cd.LensIsSpherical_ << endl;
  os << "zoomCamera_ = " << cd.zoomCamera_ << endl;
  os << "width_ = " << cd.width_ << endl;
  os << "height_ = " << cd.height_ << endl;
  for (int i=0; i<(int)cd.knownparams_vect_.size();i++) {
    os << "knownparams_vect_[" << i << "] = " << endl;
    CPDiscreteParam dp;
    dp=cd.knownparams_vect_[i];
    os << "  zoom = " << dp.zoom << endl;
    os << "  focallength = " << dp.focallength << endl;
    os << "  kc1 = " << dp.kc1;
    os << ",  kc2 = " << dp.kc2;
    os << ",  kc3 = " << dp.kc3;
    os << ",  kc4 = " << dp.kc4;
  };
  os << "MaxCamAngle_ = " << cd.MaxCamAngle_ << endl;
  os << "  acCoeff0_ = " << cd.acCoeff0_;
  os << ",  acCoeff1_ = " << cd.acCoeff1_;
  os << ",  acCoeff2_ = " << cd.acCoeff2_;
  os << ",  acCoeff3_ = " << cd.acCoeff3_;
  os << ",  acCoeff4_ = " << cd.acCoeff4_;
  os << endl;
  os << "AngleCorrX_ = " ;
  for (int i=0; i<(int) cd.AngleCorrX_.size();i++) {
    os << cd.AngleCorrX_[i] << ", ";
  }
  os << endl;
  os << "AngleCorrY_ = ";
  for (int i=0; i<(int) cd.AngleCorrY_.size();i++) {
    os << cd.AngleCorrY_[i] << ", ";
  }
  os << endl;
  os << "IlluCorrX_ = ";
  for (int i=0; i<(int) cd.IlluCorrX_.size();i++) {
    os << cd.IlluCorrX_[i] << ", ";
  }
  os << endl;
  os << "IlluCorrY_ = ";
  for (int i=0; i<(int) cd.IlluCorrY_.size();i++) {
    os << cd.IlluCorrY_[i] << ", ";
  }
  os << endl;

  os << "aspectratio_ = " << cd.aspectratio_ << endl;
  os << "cellSizeX_ = " << cd.cellSizeX_ << endl;
  os << "cellSizeY_ = " << cd.cellSizeY_ << endl;
  os << "principalX_ = " << cd.principalX_ << endl;
  os << "principalY_ = " << cd.principalY_ << endl;
  os << "focallengthDef_ = " << cd.focallengthDef_ << endl;
  os << "kc1Def_ = " << cd.kc1Def_ << endl;
  os << "kc2Def_ = " << cd.kc2Def_ << endl;
  os << "kc3Def_ = " << cd.kc3Def_ << endl;
  os << "kc4Def_ = " << cd.kc4Def_ << endl;
  os << "PoseInRigCenter_ = " << cd.PoseInRigCenter_ << endl;
  os << "PoseInRigYaw_ = " << cd.PoseInRigYaw_ << endl;
  os << "PoseInRigPitch_ = " << cd.PoseInRigPitch_ << endl;
  os << "PoseInRigRoll_ = " << cd.PoseInRigRoll_ << endl;

  return os;
}


std::ostream& operator<<(std::ostream& os,
                         BIAS::SensorData& sd) {

  os << "PoseInRigCenter_ = " << sd.PoseInRigCenter_ << endl;
  os << "PoseInRigYaw_ = " << sd.PoseInRigYaw_ << endl;
  os << "PoseInRigPitch_ = " << sd.PoseInRigPitch_ << endl;
  os << "PoseInRigRoll_ = " << sd.PoseInRigRoll_ << endl;

  return os;
}