ProjectionParametersPerspectiveDepth.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/ProjectionParametersPerspectiveDepth.hh"
#include "MathAlgo/PolynomialSolve.hh"
#include <Base/Image/ImageIO.hh>

// backward compatibility to gcc < 4.1, all other must have omp.h !
#ifdef BIAS_HAVE_OPENMP
#  if  !defined(_OPENMP) && defined(WIN32)
#    error Please check your OpenMP flags and defines - seem inconsistent.
#  endif
#  include <omp.h>
#endif


using namespace BIAS;
using namespace std;

ProjectionParametersPerspectiveDepth::
ProjectionParametersPerspectiveDepth(const unsigned int width,
                                     const unsigned int height)
  : ProjectionParametersPerspective(width, height)
{
  Init();
}


ProjectionParametersPerspectiveDepth::
ProjectionParametersPerspectiveDepth(BIAS_TOF_DEPTH_ERROR_MODEL model)
  : ProjectionParametersPerspective()
{
  Init();

  DepthCalibrationModel_ = model;

  switch(model){
  case DEPTH_MODEL_POLYNOM:
    DepthCalibrationParameters_.resize(6, 0.0);
    break;
  case DEPTH_MODEL_LINEAR:
    DepthCalibrationParameters_.resize(2, 0.0);
    break;
  case DEPTH_MODEL_SPLINE:
    // 12 control points for spline
    DepthCalibrationParameters_.resize(nrOfSplineParameters_, 0.0); 
    listSplineKnotPoints_.resize(nrOfSplineParameters_, 0.0);

    //set first to minDepth_ 
    for(unsigned i=0;i<nrOfSplineParameters_-1;i++){
      listSplineKnotPoints_[i]=DepthCalibrationParameters_[i]=
        (minDepth_ + double(i)*(maxDepth_-minDepth_)/double(nrOfSplineParameters_-3));    
    }
    //set last to maxDepth_
    listSplineKnotPoints_[nrOfSplineParameters_-1]=
      DepthCalibrationParameters_[nrOfSplineParameters_-1] = maxDepth_;
    break;
  case DEPTH_MODEL_NONE:
  case DEPTH_MODEL_UNDEFINED:
    DepthCalibrationParameters_.resize(0,0.0);
  default:
    //    BIASWARN("Depth error model None set!");
    return;
  }
}

void ProjectionParametersPerspectiveDepth::
Init(){
    D_DEPTH_DISTORTION=NewDebugLevel("D_DEPTH_DISTORTION");
    //AddDebugLevel(D_DEPTH_DISTORTION);

  DepthCalibrationModel_ = DEPTH_MODEL_POLYNOM;
  nrOfReflectivityParameters_ = 4;
  bSplinePrepared_ = false;
  nrOfSplineParameters_ = 12;
  DepthCalibrationParameters_.resize(6, 0.0);
  ReflectivityCalibrationParameters_.resize(nrOfReflectivityParameters_, 0.0);
  listSplineKnotPoints_.resize(nrOfSplineParameters_, 0.0);
  minDepth_ = 0.0; // set 0.0 meters as initial minDepth;
  maxDepth_ = 15000.0; //Set 15 meters as initial maxDepth;
}

void ProjectionParametersPerspectiveDepth::
SetDepthCalibrationParameters (const std::vector<double>& params,
                               float minDepth, 
                               float maxDepth) {
  minDepth_ = minDepth;
  maxDepth_ = maxDepth;
  unsigned size = 0;
  if(minDepth == maxDepth) 
    BIASERR("Min and Max Depth are the same! Check your parameters!"); 
  
  switch(DepthCalibrationModel_){
  case DEPTH_MODEL_POLYNOM:
    size = 6;
    if(params.size()!=6){
        BIASWARNONCE("Polynom parameters have wrong size:"<<params.size()
            <<" Check your parameters!");       
    }
    break;
  case DEPTH_MODEL_LINEAR:
    size = 2;
    if(params.size()!=2){    
        BIASWARNONCE("Linear parameters have wrong size:"<<params.size()
            <<" Check your parameters!");
    }
    break;
  case DEPTH_MODEL_SPLINE:
    size = 12;
    // we asume 10 control points! But internally have 12!
    if(params.size()!=nrOfSplineParameters_){
      BIASWARNONCE("Spline parameters have wrong size:"<<params.size()
            <<" Check your parameters!");
    }
    break;
  case DEPTH_MODEL_NONE:
  case DEPTH_MODEL_UNDEFINED:
    size = 0;
    return;
  default:
    //BIASWARN("Depth error model None set!");
    return;
  }
  //set params
  DepthCalibrationParameters_.clear();
  for(unsigned i=0;i<size;i++){
    DepthCalibrationParameters_.push_back(params[i]);
  }

  if(DepthCalibrationModel_==DEPTH_MODEL_SPLINE){   
    listSplineKnotPoints_.clear();
    listSplineKnotPoints_.resize(nrOfSplineParameters_,0.0);

    //set first to 0 
    listSplineKnotPoints_[0]=DepthCalibrationParameters_[0];
 
    for(unsigned i=1;i<nrOfSplineParameters_-1;i++){
      listSplineKnotPoints_[i]=(minDepth + double(i-1)*(maxDepth-minDepth)/
                                double(nrOfSplineParameters_-3));
     
    }
    //set last knot point to last depth point
    listSplineKnotPoints_[nrOfSplineParameters_-1]=
      DepthCalibrationParameters_[nrOfSplineParameters_-1];
    
    PrepareSpline_();     
  }
}

void ProjectionParametersPerspectiveDepth::
SetDepthCalibrationParametersSpline (const std::vector<double>& knots,
                                     const std::vector<double>& controls,
                                     float minDepth, 
                                     float maxDepth) {
  BIASASSERT(DepthCalibrationModel_ == DEPTH_MODEL_SPLINE);
  BIASASSERT(knots.size() == controls.size());
  BIASASSERT(knots.size() == nrOfSplineParameters_);
  BIASASSERT(minDepth_ == maxDepth_);
  minDepth_ = minDepth;
  maxDepth_ = maxDepth;
  listSplineKnotPoints_ = knots;
  DepthCalibrationParameters_ = controls;
  PrepareSpline_();
}


int ProjectionParametersPerspectiveDepth::
SetReflectivityNormalizationParameters(const std::vector<double>& depthValues,
                                    const std::vector<double>& minInt, 
                                    const std::vector<double>& maxInt,
                                    const std::vector<double>& radiusValues,
                                    const std::vector<double>& radiusIntensities){
    int ret = 0;
    std::vector<double> minIntEval;
    std::vector<double> maxIntEval;

    depthIntNormValues_.clear();
  for(unsigned i=0 ; i< depthValues.size(); i++){
    if(minInt[i]<maxInt[i]){
      depthIntNormValues_.push_back(depthValues[i]);
      minIntEval.push_back(minInt[i]);
      maxIntEval.push_back(maxInt[i]);
    }
  }
  if(depthIntNormValues_.size() > 0){
    PolynomialSolve PS;
    ret = PS.FitPolynomial(3, depthIntNormValues_, minIntEval, minNormIntCoeffs_);
    if(ret < 0) return ret;
    ret = PS.FitPolynomial(3, depthIntNormValues_, maxIntEval, maxNormIntCoeffs_);
    if(ret < 0) return ret;
    /*
    cout<<"minNormCoeffs:";
    for(unsigned i=0;i<minNormIntCoeffs_.size();i++) cout<<minNormIntCoeffs_[i]<<"\t";
    cout<<endl;
    cout<<"maxNormCoeffs:";
    for(unsigned i=0;i<maxNormIntCoeffs_.size();i++) cout<<maxNormIntCoeffs_[i]<<"\t";
    cout<<endl;
    */
  }
  //  else
  //   BIASERR("SetReflectivityNormalizationParameters failed, no values");

  //Determine the valid entries in radius vectors
  std::vector<double> radiusEval;
  radiusIntNormValues_.clear();
  for(unsigned i=0 ; i< radiusValues.size(); i++){
    if(radiusIntensities[i] != 0.0){
      radiusIntNormValues_.push_back(radiusValues[i]);
      radiusEval.push_back(radiusIntensities[i]);     
    }
  }
  //fit Polynomial for radius dependent vignetting
  if(radiusIntNormValues_.size() > 0){
    PolynomialSolve PS;
    ret = PS.FitPolynomial(3, radiusIntNormValues_, radiusEval, radiusIntNormCoeffs_);
    if(ret < 0) return ret;
  }
  return ret;
}

void ProjectionParametersPerspectiveDepth::
SetReflectivityCalibrationParameters (const std::vector<double>& params){
   ReflectivityCalibrationParameters_ = params;
}

void ProjectionParametersPerspectiveDepth::
SetDepthCalibrationModel(BIAS_TOF_DEPTH_ERROR_MODEL model) {
    DepthCalibrationModel_= model;
    switch(model){
    case DEPTH_MODEL_POLYNOM:
        DepthCalibrationParameters_.resize(6, 0.0);
        break;
    case DEPTH_MODEL_LINEAR:
        DepthCalibrationParameters_.resize(2, 0.0);
        break;
    case DEPTH_MODEL_SPLINE:
        // 12 control points for spline
        DepthCalibrationParameters_.resize(nrOfSplineParameters_, 0.0);
        listSplineKnotPoints_.resize(nrOfSplineParameters_, 0.0);

        //set first to minDepth_
        for(unsigned i=0;i<nrOfSplineParameters_-1;i++){
            listSplineKnotPoints_[i]=DepthCalibrationParameters_[i]=
                    (minDepth_ + double(i)*(maxDepth_-minDepth_)/double(nrOfSplineParameters_-3));
        }
        //set last to maxDepth_
        listSplineKnotPoints_[nrOfSplineParameters_-1]=
                DepthCalibrationParameters_[nrOfSplineParameters_-1] = maxDepth_;
        break;
    case DEPTH_MODEL_NONE:
    case DEPTH_MODEL_UNDEFINED:
    default:
        DepthCalibrationParameters_.resize(0, 0.0);
        break;
    }
}


void ProjectionParametersPerspectiveDepth::
PrepareSpline_(){
  splineInterpolator_.Clear();
  splineInterpolator_.SetKnotPoints(listSplineKnotPoints_);
  splineInterpolator_.SetControlPoints(DepthCalibrationParameters_);
  splineInterpolator_.InitSpline(); 
  splineInterpolatorUndist_.Clear();
  splineInterpolatorUndist_.SetKnotPoints(DepthCalibrationParameters_);
  splineInterpolatorUndist_.SetControlPoints(listSplineKnotPoints_);
  splineInterpolatorUndist_.InitSpline(); 
  bSplinePrepared_ = true;
}


void ProjectionParametersPerspectiveDepth::
DistortDepth (HomgPoint2D const &pos, float &d, bool bIsInCartesianCoords) {

    if(DepthCalibrationModel_== DEPTH_MODEL_NONE) return;

    BIASDOUT(D_DEPTH_DISTORTION,"DistortDepth, d orig:"<<d)
    if(!bIsInCartesianCoords)
        d = TransformPolarToCartesianCoordinates(pos,d);
    BIASDOUT(D_DEPTH_DISTORTION,"DistortDepth, d cartesian:"<<d)

#ifdef BIAS_DEBUG
  if(BIAS_ISNAN((double)d) || BIAS_ISINF((double)d)){
        BIASWARN("Error:isnan or isinf failed for "<<d);
        d=0;
    }
#endif

    switch(DepthCalibrationModel_){
  case DEPTH_MODEL_POLYNOM:
    d = float(DepthCalibrationParameters_[0])
      + (1.0f+float(DepthCalibrationParameters_[1]))*d
      + float(DepthCalibrationParameters_[2]*pos[0]/pos[2])
      + float(DepthCalibrationParameters_[3]*pos[1]/pos[2])
      + float(DepthCalibrationParameters_[4])*d*d
      + float(DepthCalibrationParameters_[5])*d*d*d;
    break;
  case DEPTH_MODEL_LINEAR:
    d = float(DepthCalibrationParameters_[0])
      + (1.0f+float(DepthCalibrationParameters_[1]))*d;
    break;
  case  DEPTH_MODEL_SPLINE:
    {
      if (!bSplinePrepared_) PrepareSpline_();
      double corr=0.0;
      splineInterpolator_.Spline(corr,(double)d,4); // akima spline  
      d = float(corr);
      break;
    }
  case DEPTH_MODEL_NONE:
  case DEPTH_MODEL_UNDEFINED:
  default:
    //BIASWARN("Depth error model None set!");
    break;
  }

    BIASDOUT(D_DEPTH_DISTORTION,"DistortDepth, d cartesian distorted:"<<d)
  if(!bIsInCartesianCoords)
    d = TransformCartesianToPolarCoordinates(pos,d);
    BIASDOUT(D_DEPTH_DISTORTION,"DistortDepth, d polar distorted:"<<d)
}


void ProjectionParametersPerspectiveDepth::
UnDistortDepth (HomgPoint2D const &pos, float &d,bool bIsInCartesianCoords){

    if(DepthCalibrationModel_== DEPTH_MODEL_NONE) return;
  if (d == 0.0) return;

  std::vector<double> poly(4);
  std::vector<double> sol;
  PolynomialSolve solver;
  double dd;

  if(!bIsInCartesianCoords)
    d = TransformPolarToCartesianCoordinates(pos,d);

  switch(DepthCalibrationModel_){

  case DEPTH_MODEL_POLYNOM:
    poly[0] = DepthCalibrationParameters_[0]
      + DepthCalibrationParameters_[2]*pos[0]/pos[2]
      + DepthCalibrationParameters_[3]*pos[1]/pos[2]
      - d;
    poly[1] = (1+DepthCalibrationParameters_[1]);
    poly[2] = DepthCalibrationParameters_[4];
    poly[3] = DepthCalibrationParameters_[5];
    solver.Solve (poly, sol);
    
    BIASASSERT(sol.size()>0);
    dd=sol[0];
    for (unsigned int i=1; i<sol.size(); i++) {
      if (fabs(sol[i]-d)<fabs(dd-d))
        dd = sol[i];
    }
    d = float(dd);
    break;
  case  DEPTH_MODEL_LINEAR:
    d = (d-float(DepthCalibrationParameters_[0]))/
      (1.0f+float(DepthCalibrationParameters_[1]));
    break;
  case  DEPTH_MODEL_SPLINE:
    {
      if (!bSplinePrepared_) PrepareSpline_();
      double corr=0.0;
      //backward spline function
      splineInterpolatorUndist_.Spline(corr,(double)d,4); // akima spline  
      d = float(corr);
      break;
    }
  case DEPTH_MODEL_NONE:
  case DEPTH_MODEL_UNDEFINED:
  default:
    //    BIASWARN("Depth error model None set!");
    break;
  }
  if(!bIsInCartesianCoords)
    d = TransformCartesianToPolarCoordinates(pos,d);
}

int ProjectionParametersPerspectiveDepth::
UnDistortDepthMapIP(BIAS::Image<float>& depthMap, bool bIsInCartesianCoords)
{
  if(!HasDepthDistortion()) return 0;

#ifdef BIAS_DEBUG
  unsigned int width = depthMap.GetWidth();
  unsigned int height = depthMap.GetHeight();
  unsigned int channels = depthMap.GetChannelCount();
  if(width_ != width || height_ != height || channels != 1)
    return -1;
#endif

  Image<float> tmpCartDepth;
  if(!bIsInCartesianCoords){
    TransformPolarToCartesianCoordinates(depthMap,tmpCartDepth);
    //ImageIO::Save("cartesian-depth.mip",tmpCartDepth);
  }
  else
    tmpCartDepth = depthMap;

  float **depthPtr = tmpCartDepth.GetImageDataArray();
  HomgPoint2D p;
  p[2] = 1.0;
  for (unsigned int y=0;y<height_;y++) {
    for (unsigned int x=0;x<width_;x++) {
      p[0] = static_cast<float>(x);
      p[1] = static_cast<float>(y);
      //give true as last argument because we transformed
      //the whole image to cartesian coordinates
      UnDistortDepth(p, depthPtr[y][x],true);
    }
  }
  //ImageIO::Save("cartesian-depth-undist.mip",tmpCartDepth);
  if(!bIsInCartesianCoords)
    TransformCartesianToPolarCoordinates(tmpCartDepth,depthMap);
  else
    depthMap=tmpCartDepth;
  return 0;
}

int ProjectionParametersPerspectiveDepth::
DistortDepthMapIP(BIAS::Image<float>& depthMap, bool bIsInCartesianCoords)
{
  if(!HasDepthDistortion()) return 0;
#ifdef BIAS_DEBUG
  unsigned int width = depthMap.GetWidth();
  unsigned int height = depthMap.GetHeight();
  unsigned int channels = depthMap.GetChannelCount();
  if(width_ != width || height_ != height || channels != 1)
    return -1;
#endif
  Image<float> tmpCartDepth;
  if(!bIsInCartesianCoords){
    TransformPolarToCartesianCoordinates(depthMap,tmpCartDepth);
    //ImageIO::Save("cartesian-depth.mip",tmpCartDepth);
  }
  else
    tmpCartDepth = depthMap;

  float **depthPtr = tmpCartDepth.GetImageDataArray();
  HomgPoint2D p;
  p[2] = 1.0;
  for (unsigned int y=0;y<height_;y++) {
    for (unsigned int x=0;x<width_;x++) {
      p[0] = static_cast<float>(x);
      p[1] = static_cast<float>(y);
      //give true as last argument because we transformed
      //the whole image to cartesian coordinates
      DistortDepth(p, depthPtr[y][x],true);
    }
  }
  //ImageIO::Save("cartesian-depth-dist.mip",tmpCartDepth);
  if(!bIsInCartesianCoords)
    TransformCartesianToPolarCoordinates(tmpCartDepth,depthMap);
  else
    depthMap=tmpCartDepth;
  return 0;
}
//////////////////////////////////////////////////////////////////////////////////////

/* @brief Distort Reflectivity value depth at image position pos */
void ProjectionParametersPerspectiveDepth::
DistortReflectivity (HomgPoint2D const &pos,float &depth,
        float &reflectivity,bool bIsInCartesianCoords){

  double px=0,py=0; GetPrincipal(px,py);
    float radius =sqrt(float((pos[0]-px)*(pos[0]-px) +(pos[1]-py)*(pos[1]-py)));
  if(!bIsInCartesianCoords)
    depth = TransformPolarToCartesianCoordinates(pos,depth);

  //normalize Reflectivity
  double min ,max,radialOffsetOne,radialOffset;
  PolynomialSolve PS;
  min = PS.EvaluatePolynomial(depth,minNormIntCoeffs_);
  max = PS.EvaluatePolynomial(depth,maxNormIntCoeffs_);
  radialOffsetOne = PS.EvaluatePolynomial(1,radiusIntNormCoeffs_);
  radialOffset = PS.EvaluatePolynomial(radius,radiusIntNormCoeffs_);
  min +=radialOffsetOne - radialOffset; //r(1) = r(r)
  max +=radialOffsetOne - radialOffset;
  double i_norm = reflectivity-min/(max-min);
  depth = depth - float(PS.EvaluatePolynomial(i_norm,ReflectivityCalibrationParameters_));

  if(!bIsInCartesianCoords)
    depth = TransformCartesianToPolarCoordinates(pos,depth);
}

/* @brief UnDistort Reflectivity value i at image position pos */
void  ProjectionParametersPerspectiveDepth::
UnDistortReflectivity (HomgPoint2D const &pos,float &depth,
        float &reflectivity,bool bIsInCartesianCoords){

  double px=0,py=0; GetPrincipal(px,py);
    float radius =sqrt(float((pos[0]-px)*(pos[0]-px) +(pos[1]-py)*(pos[1]-py)));
  if(!bIsInCartesianCoords)
    depth = TransformPolarToCartesianCoordinates(pos,depth);

  double min ,max,radialOffsetOne,radialOffset;
  PolynomialSolve PS;
  min = PS.EvaluatePolynomial(depth,minNormIntCoeffs_);
  max = PS.EvaluatePolynomial(depth,maxNormIntCoeffs_);
  radialOffsetOne = PS.EvaluatePolynomial(1,radiusIntNormCoeffs_);
  radialOffset = PS.EvaluatePolynomial(radius,radiusIntNormCoeffs_);
  min +=radialOffsetOne - radialOffset; //r(1) = r(r)
  max +=radialOffsetOne - radialOffset;
  double i_norm = reflectivity-min/(max-min);
  depth = depth + float(PS.EvaluatePolynomial(i_norm,ReflectivityCalibrationParameters_));

  if(!bIsInCartesianCoords)
    depth = TransformCartesianToPolarCoordinates(pos,depth);
}

/* @brief Distort Reflectivity image in-place */
int ProjectionParametersPerspectiveDepth::
DistortReflectivityDepthIP(BIAS::Image<float>& depthMap, 
                        BIAS::Image<float>& ReflectivityImage,
                        bool bIsInCartesianCoords){

    Image<float> tmpCartDepth;
    if(!bIsInCartesianCoords)
        TransformPolarToCartesianCoordinates(depthMap,tmpCartDepth);
    else
        tmpCartDepth = depthMap;

  int width = (int)tmpCartDepth.GetWidth();
  int height = (int)tmpCartDepth.GetHeight();
  float **depthPtr = tmpCartDepth.GetImageDataArray();
  float **intPtr = ReflectivityImage.GetImageDataArray();

// #ifdef BIAS_HAVE_OPENMP
// #pragma omp parallel for if(omp_get_num_procs() > 1)  shared(depthPtr,intPtr)
// #endif
  for (int y=0;y<height;y++) {
    for (int x=0;x<width;x++) {
        HomgPoint2D pos(x,y,1);
      DistortReflectivity(pos,depthPtr[y][x],intPtr[y][x],true);
    }
  }
  if(!bIsInCartesianCoords)
    TransformCartesianToPolarCoordinates(tmpCartDepth,depthMap);
  else
    depthMap=tmpCartDepth;
  return 0;
}

/* @brief Undistort Reflectivity image in-place */
int ProjectionParametersPerspectiveDepth::
UnDistortReflectivityDepthIP(BIAS::Image<float>& depthMap, 
                             BIAS::Image<float>& ReflectivityImage,
                             bool bIsInCartesianCoords){

    Image<float> tmpCartDepth;
    if(!bIsInCartesianCoords)
        TransformPolarToCartesianCoordinates(depthMap,tmpCartDepth);
    else
        tmpCartDepth = depthMap;
  int width = (int)tmpCartDepth.GetWidth();
  int height = (int)tmpCartDepth.GetHeight();
  float **depthPtr = tmpCartDepth.GetImageDataArray();
  float **intPtr = ReflectivityImage.GetImageDataArray();

// #ifdef BIAS_HAVE_OPENMP
// #pragma omp parallel for if(omp_get_num_procs() > 1)  shared(depthPtr,intPtr)
// #endif
  for (int y=0;y<height;y++) {
    for (int x=0;x<width;x++) {
        HomgPoint2D pos(x,y,1);
      UnDistortReflectivity(pos,depthPtr[y][x],intPtr[y][x],true);
    }
  }
  if(!bIsInCartesianCoords)
    TransformCartesianToPolarCoordinates(tmpCartDepth,depthMap);
  else
    depthMap=tmpCartDepth;
  return 0;
}


bool ProjectionParametersPerspectiveDepth::
HasDepthDistortion() const
{
  bool hasNoDepthDistortion = true;
  for(unsigned int i=0; i<DepthCalibrationParameters_.size(); i++) {
    hasNoDepthDistortion = (hasNoDepthDistortion && DepthCalibrationParameters_[i] == 0.0 );
  }
  return !hasNoDepthDistortion;
}

bool ProjectionParametersPerspectiveDepth::
HasReflectivityDistortion() const
{
  bool hasNoReflectivityDistortion = true;
  for(unsigned int i=0; i<nrOfReflectivityParameters_; i++) {
    hasNoReflectivityDistortion=(hasNoReflectivityDistortion && 
                              ReflectivityCalibrationParameters_[i]==0.0);
  }
  if(minNormIntCoeffs_.size() != 4 || maxNormIntCoeffs_.size()!= 4)
    hasNoReflectivityDistortion = true;
  return !hasNoReflectivityDistortion;
}

void ProjectionParametersPerspectiveDepth::
SetDepthDistortionToZero()
{
  for(unsigned int i=0; i<DepthCalibrationParameters_.size(); i++) {
    DepthCalibrationParameters_[i] = 0.0 ;
  }
  
  if(DepthCalibrationModel_==DEPTH_MODEL_NONE){   
    listSplineKnotPoints_.clear();
    listSplineKnotPoints_.resize(nrOfSplineParameters_, 0.0);
    for(unsigned i=0;i<nrOfSplineParameters_;i++)
      listSplineKnotPoints_[i]=0.0;
  }
}

void ProjectionParametersPerspectiveDepth::
SetReflectivityDepthDistortionToZero()
{
  for(unsigned int i=0; i<nrOfReflectivityParameters_; i++) {
    ReflectivityCalibrationParameters_[i] = 0.0 ;
  }
}

unsigned  ProjectionParametersPerspectiveDepth::
GetNrOfDepthErrormodelParameters(BIAS_TOF_DEPTH_ERROR_MODEL model)
{   
    if(model == DEPTH_MODEL_NONE ||
         model ==DEPTH_MODEL_UNDEFINED)
        return 0;
    if(model == DEPTH_MODEL_LINEAR)
        return 2;
    if(model == DEPTH_MODEL_POLYNOM)
        return 6;
    if(model == DEPTH_MODEL_SPLINE)
    return nrOfSplineParameters_-2;
    else return 0;
}

unsigned ProjectionParametersPerspectiveDepth::
GetNrOfReflectivityParameters()
{
    return nrOfReflectivityParameters_;
}


std::string ProjectionParametersPerspectiveDepth::
GetDepthDistModelString(BIAS_TOF_DEPTH_ERROR_MODEL model){
    switch(model){
    case DEPTH_MODEL_NONE:
        return string("None");
    case DEPTH_MODEL_LINEAR:
        return string("Linear");
    case DEPTH_MODEL_POLYNOM:
        return string("Polynom");
    case DEPTH_MODEL_SPLINE:
        return string("Spline");
    case DEPTH_MODEL_UNDEFINED:
            return string("Undefined");
    default:
        return string("Unknown");
    }
}

#ifdef BIAS_HAVE_XML2

int ProjectionParametersPerspectiveDepth::
XMLGetClassName(std::string& TopLevelTag, double& Version) const {
  TopLevelTag = "ProjectionParametersPerspectiveDepth";
  Version = 0.1;
  return 0;
}

int ProjectionParametersPerspectiveDepth::
XMLOut(const xmlNodePtr Node, XMLIO& XMLObject) const {
  ProjectionParametersPerspective::XMLOut (Node, XMLObject);
  xmlNodePtr childNode;
  childNode = XMLObject.addChildNode(Node,"DepthCalibration");
  switch(DepthCalibrationModel_){
  case DEPTH_MODEL_POLYNOM:
    XMLObject.addAttribute(childNode,"Model","DEPTH_MODEL_POLYNOM");
    break;
  case DEPTH_MODEL_LINEAR:
    XMLObject.addAttribute(childNode,"Model","DEPTH_MODEL_LINEAR");
    break;
  case DEPTH_MODEL_SPLINE:
    XMLObject.addAttribute(childNode,"Model","DEPTH_MODEL_SPLINE");
    XMLObject.addAttribute(childNode,"NrSplineParameters",(int)nrOfSplineParameters_);
    break;
  case DEPTH_MODEL_NONE:
    XMLObject.addAttribute(childNode,"Model","DEPTH_MODEL_NONE");
    break;
  case DEPTH_MODEL_UNDEFINED:
    XMLObject.addAttribute(childNode,"Model","DEPTH_MODEL_UNDEFINED");
    break;
  default:   
    break;
  }
  for (unsigned int i=0; i<DepthCalibrationParameters_.size(); i++) {
    std::ostringstream o;
    o << "d" << i;
    XMLObject.addAttribute(childNode, o.str(), DepthCalibrationParameters_[i]);
  }

  if(DepthCalibrationModel_ == DEPTH_MODEL_SPLINE){
    for (unsigned int i=0; i<listSplineKnotPoints_.size(); i++) {
      std::ostringstream o;
      o << "k" << i;
      XMLObject.addAttribute(childNode, o.str(), listSplineKnotPoints_[i]);
    }
  }

  XMLObject.addAttribute(childNode,"MinDepth",minDepth_);
  XMLObject.addAttribute(childNode,"MaxDepth",maxDepth_);
  
  childNode = XMLObject.addChildNode(Node,"ReflectivityCalibration");
  for (unsigned int i=0; i<nrOfReflectivityParameters_; i++) {
    std::ostringstream o;
    o << "i" << i;
    XMLObject.addAttribute(childNode, o.str(), ReflectivityCalibrationParameters_[i]);
  }
  for (unsigned int i=0; i<minNormIntCoeffs_.size(); i++) {
    std::ostringstream o;
    o << "cmin" << i;
    XMLObject.addAttribute(childNode, o.str(), minNormIntCoeffs_[i]);
  }
  for (unsigned int i=0; i<maxNormIntCoeffs_.size(); i++) {
    std::ostringstream o;
    o << "cmax" << i;
    XMLObject.addAttribute(childNode, o.str(), maxNormIntCoeffs_[i]);
  }

  for (unsigned int i=0; i<radiusIntNormCoeffs_.size(); i++) {
    std::ostringstream o;
    o << "crad" << i;
    XMLObject.addAttribute(childNode, o.str(), radiusIntNormCoeffs_[i]);
  }
  return 0;
}

int ProjectionParametersPerspectiveDepth::
XMLIn(const xmlNodePtr Node, XMLIO& XMLObject) {
  ProjectionParametersPerspective::XMLIn(Node, XMLObject);
  xmlNodePtr childNode;

  if ((childNode = XMLObject.getChild(Node, "DepthCalibration"))!=NULL) {
    xmlAttrPtr modelAttr = NULL;
    modelAttr = XMLObject.getAttributeByName(childNode, "Model");
    if(modelAttr!=NULL)
    {
        string model;
        model = XMLObject.getAttributeValueString(modelAttr);
        if(model == "DEPTH_MODEL_POLYNOM"){
            DepthCalibrationParameters_.resize(6, 0.0);
            DepthCalibrationModel_ = DEPTH_MODEL_POLYNOM;
        }
        else if(model == "DEPTH_MODEL_LINEAR"){
            DepthCalibrationParameters_.resize(2, 0.0);
            DepthCalibrationModel_ = DEPTH_MODEL_LINEAR;
        }
        else if(model == "DEPTH_MODEL_SPLINE"){
            modelAttr = NULL;
            modelAttr =  XMLObject.getAttributeByName(childNode, "NrSplineParameters");
            if(modelAttr!=NULL){
                nrOfSplineParameters_ = XMLObject.getAttributeValueInt(modelAttr);
            }
            DepthCalibrationParameters_.resize(nrOfSplineParameters_, 0.0);
            listSplineKnotPoints_.resize(nrOfSplineParameters_, 0.0);
            DepthCalibrationModel_ = DEPTH_MODEL_SPLINE;
        }
        else if(model == "DEPTH_MODEL_NONE"){
            DepthCalibrationModel_ = DEPTH_MODEL_NONE;
        }
        else if(model == "DEPTH_MODEL_UNDEFINED"){
            DepthCalibrationModel_ = DEPTH_MODEL_UNDEFINED;
        }
    }
    else
      DepthCalibrationParameters_.resize(6, 0.0);// standard case Polynom = length 6

    for (unsigned int i=0; i<DepthCalibrationParameters_.size(); i++) {
      std::ostringstream o;
      o << "d" << i;
      DepthCalibrationParameters_[i] = XMLObject.getAttributeValueDouble(childNode, o.str());
    }
    if(DepthCalibrationModel_ == DEPTH_MODEL_SPLINE){
      for (unsigned int i=0; i<listSplineKnotPoints_.size(); i++) {
        std::ostringstream o;
        o << "k" << i;
        listSplineKnotPoints_[i] = XMLObject.getAttributeValueDouble(childNode, o.str());
      }
    }

    double min, max;
    min = XMLObject.getAttributeValueDouble(childNode, "MinDepth");
    max = XMLObject.getAttributeValueDouble(childNode, "MaxDepth");
    if(min!=max && min >=0.0 && max > 0.0)
      {minDepth_ = (float)min; maxDepth_ = (float)max;}   
  }
  if ((childNode = XMLObject.getChild(Node, "ReflectivityCalibration"))!=NULL) {
    ReflectivityCalibrationParameters_.resize(nrOfReflectivityParameters_);
    for (unsigned int i=0; i<nrOfReflectivityParameters_; i++) {
      std::ostringstream o;
      o << "i" << i;      
      ReflectivityCalibrationParameters_[i] = XMLObject.getAttributeValueDouble(childNode, o.str());
    }
    minNormIntCoeffs_.resize(4);maxNormIntCoeffs_.resize(4),radiusIntNormCoeffs_.resize(4);
    for (unsigned int i=0; i<4; i++) {
     std::ostringstream o,o2,o3;
     o << "cmin" << i;
     o2<< "cmax" << i;
     o3<< "crad" << i;
     minNormIntCoeffs_[i] = XMLObject.getAttributeValueDouble(childNode, o.str());
     maxNormIntCoeffs_[i] = XMLObject.getAttributeValueDouble(childNode, o2.str());
     radiusIntNormCoeffs_[i] = XMLObject.getAttributeValueDouble(childNode, o3.str());     
    }
  }
  
  return 0;
}


#endif // BIAS_HAVE_XML2