ProjectionParametersSpherical.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/ProjectionParametersSpherical.hh"
#include <Base/Common/CompareFloatingPoint.hh>
#include <MathAlgo/PolynomialSolve.hh>
#include <MathAlgo/Minpack.hh>

using namespace BIAS;
using namespace std;

bool ProjectionParametersSpherical::
Distort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("unfinished");
  BIASABORT;
  return false;
}


bool ProjectionParametersSpherical::
Undistort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("unfinished");
  BIASABORT;
  return false;
}


// void ProjectionParametersSpherical::GetFirstBorderPixel(PixelIterator& it) {
//   it.x = 0;
//   it.y = 0;
//   BIASERR("Please implment me for running on the outer field of view circle");
// }


// bool ProjectionParametersSpherical::GetNextBorderPixel(PixelIterator& it) {
//   BIASERR("Please implment me for running on the outer field of view circle");
//   return ProjectionParametersBase::GetNextBorderPixel(it);
// }


const bool ProjectionParametersSpherical::
DoIntrinsicsDiffer(const ProjectionParametersBase* p) const
{
  if(ProjectionParametersBase::DoIntrinsicsDiffer(p))
    return true;

  const ProjectionParametersSpherical* pps =
    dynamic_cast<const ProjectionParametersSpherical*>(p);
  if(pps == NULL)
    return true;

  if(radius_ != pps->radius_)
    return true;
  if(interpolatorInit_ != pps->interpolatorInit_)
    return true;
  if(interpolatorInit_ &&
     interpolatorAngle_.DoLuTSplinesDiffer(pps->interpolatorAngle_))
      return true;
  if(MaxCamAngle_ != pps->MaxCamAngle_)
    return true;

  return false;
}


bool ProjectionParametersSpherical::
DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                               HomgPoint2D& x,
                               bool IgnoreDistortion) const
{
  ProjectLocal(localX, x, IgnoreDistortion);
  double radius =
    sqrt( (x[0]-principalX_) * (x[0]-principalX_) +
          (x[1]-principalY_) * (x[1]-principalY_) );

  return (x[0]>=0.0 && x[1]>=0.0 &&
          x[0]<=(double)(width_-1) && x[1]<=(double)(height_-1) &&
          radius<= radius_);
}


int ProjectionParametersSpherical::
ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
             bool IgnoreDistortion) const
{
  Vector3<double> rPhiTheta = point.CoordEuclideanToSphere();
  if (Equal(rPhiTheta[0], 0.0)) {
    p2d.SetZero();
    return 0;
  }
  if (!IgnoreDistortion) {
    TransfCoordSphere2Image_(rPhiTheta[2], rPhiTheta[1], p2d);
    return 0;
  }
  else {
    TransfCoordSphere2CalibImage_(rPhiTheta[2], rPhiTheta[1], p2d);
    return 0;
  }
  return 1;
}


HomgPoint3D ProjectionParametersSpherical::
UnProjectToImagePlane(const HomgPoint2D& pos, const double& depth,
                      bool IgnoreDistortion) const
{
  Vector3<double> d, p;
  UnProjectLocal(pos, p, d, IgnoreDistortion);
  double radius = sqrt(d[2]*d[2] + d[1]*d[1] + d[0]*d[0]);
  if(Equal(radius, 0.0))
    return HomgPoint3D(0,0,0,0);
  d*= depth/radius;
  if(Equal(d.NormL2(), 0.0))
    return HomgPoint3D(0,0,0,0);
  HomgPoint3D local(d);
  local[3] = 1.0;
  return Pose_.LocalToGlobal(local);
}



void  ProjectionParametersSpherical::
UnProjectLocal(const HomgPoint2D& pos, Vector3<double>& origin,
               Vector3<double>& direction, bool IgnoreDistortion) const
{
  //radius = 1;
  //cout<<"[ProjectionParametersSpherical] UnProjectLocal "<<pos<<endl;
  Vector3<double> rPhiTheta(1,0,0);
  if (!IgnoreDistortion) {
    TransfCoordImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);
  }
  else {
    TransfCoordCalibImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);
  }
  direction = rPhiTheta.CoordSphereToEuclidean();
  origin = Vector3<double>(0.0);
}

Vector3<double> ProjectionParametersSpherical::
UnProjectToPointLocal(const HomgPoint2D& pos,
                      const double& depth,
                      bool IgnoreDistortion) const
{
  Vector3<double> x, p;
  UnProjectLocal(pos, p, x, IgnoreDistortion);
  x *= depth;
  return x;
}


int ProjectionParametersSpherical::
SetUndistortion(const std::vector<double>& undistAngles,
                const std::vector<double>& distAngles,
                const double radius)
{
  radius_ = radius;

  if (undistAngles.size() < 2 || distAngles.size() < 2 ||
      undistAngles.size() != distAngles.size()) {
    BIASERR("Initialization failed, expected min. 2 and same number of "
            "undistorted and distorted angles (given " << undistAngles.size()
            << " and " << distAngles.size() << " angles)!");
    return -1;
  }

  // set interpolator from undistorted to distorted angles
  double dummy = 0.0;
  interpolatorAngle_.SetKnotPoints(undistAngles);
  interpolatorAngle_.SetControlPoints(distAngles);
  interpolatorAngle_.InitSpline();
  interpolatorAngle_.Spline(dummy, undistAngles[0], 4);
  MaxCamAngle_ = undistAngles[undistAngles.size()-1];
  int lutsamples = int(radius_*100);
  interpolatorAngle_.PrepareLuTSpline(undistAngles[0], MaxCamAngle_,
                                      lutsamples, 4);

  // set interpolator from distorted to undistorted angles
  interpolatorAngleBack_.SetKnotPoints(distAngles);
  interpolatorAngleBack_.SetControlPoints(undistAngles);
  interpolatorAngleBack_.InitSpline();
  interpolatorAngleBack_.Spline(dummy, distAngles[0], 4);
  interpolatorAngleBack_.PrepareLuTSpline(distAngles[0],
                                          distAngles[distAngles.size()-1],
                                          lutsamples, 4);
  interpolatorInit_ = true;

  return 0;
}


void ProjectionParametersSpherical::
SetMaxCamAngle(const double maxCamAngle)
{
  if (interpolatorInit_) {
    BIASWARN("Recalculating undistortion polynomial since max. camera "
             "angles has been reset manually!");
    RecalculateUndistortion(maxCamAngle);
  } else {
    MaxCamAngle_ = maxCamAngle;
  }
}


void ProjectionParametersSpherical::
RecalculateUndistortion(const double maxCamAngle)
{
  if (!interpolatorInit_) {
    BEXCEPTION("Undistortion polynomial has not been set previously! "
               "Call SetUndistortion instead of RecalculateUndistortion!");
  }
  if (maxCamAngle > MaxCamAngle_) {
    BIASWARN("Extrapolating undistortion polynomial linearly since new "
             "max. cam angle > old max. cam angle. Might result in strange "
             "behaviour!");
  }
  std::vector<double> dummy;
  interpolatorAngle_.GetKnotPoints(dummy);
  const unsigned int numSamples = dummy.size();
  std::vector<double> undistAngles(numSamples);
  std::vector<double> distAngles(numSamples);
  double distMaxCamAngle = MaxCamAngle_; // current distorted max. camera angle
  interpolatorAngle_.Spline(distMaxCamAngle, MaxCamAngle_, 4);
  const double linearExtrapolationFactor = distMaxCamAngle / MaxCamAngle_;
  double theta = 0; // undistorted angle samples
  const double thetaStep = maxCamAngle / (numSamples - 1);
  for (unsigned int i = 0; i < numSamples; i++, theta += thetaStep) {
    undistAngles[i] = theta;
    // compute distorted angle sample
    if (theta < MaxCamAngle_) {
      interpolatorAngle_.Spline(distAngles[i], theta, 4);
    } else {
      // extrapolate angle linearly
      distAngles[i] = theta * linearExtrapolationFactor;
    }
  }
  // recalculate polynomial interpolation and set new max. camera angle
  SetUndistortion(undistAngles, distAngles, radius_);
}


int ProjectionParametersSpherical::
InitAngleCorrFromPoly(const double maxCamAngle,
                      const std::vector<double>& coefficients,
                      const bool mapsRadiusToZ)
{
  double pix_def, ang_def, ang_opt;

  // calculate distortion function
  std::vector<double> radiusInPixel, thetaInRad;
  double maxradius = 0;
  MaxCamAngle_ = maxCamAngle;

  acCoeff_ = coefficients;
  std::vector<double> distAngles;
  std::vector<double> undistAngles;
  unsigned int width, height;
  GetImageSize(width, height);
  PolynomialSolve PS; // helper object to evaluate polynomial

  unsigned int rad = radius_ != 0.0 ? (unsigned int)rint(radius_) :
                                      (unsigned int)rint(double(width)/2.0);

  // Compute samples for distorted and corresponding undistorted
  // azimuth angles to interpolate undistortion polynomial from
  if (mapsRadiusToZ)
  {
    // run radially from distortion center to sphere border
    for (unsigned int currentRadius = 0; currentRadius <= rad; currentRadius++)
    {
      // pick radius
      radiusInPixel.push_back(currentRadius);
      // compute z coordinate such that (currentRadius, 0, -z) is ray in
      // the camera coordinate system (this is the Scaramuzza polynomial)
      double z = PS.EvaluatePolynomial(currentRadius, acCoeff_);
      // compute theta for the radius
      double currentTheta = atan2(currentRadius, -z);
      thetaInRad.push_back(currentTheta);
      // check if we have to increase the radius
      if (currentRadius > maxradius && currentTheta < MaxCamAngle_)
        maxradius = currentRadius;
    }
  } else {
    // run radially from distortion center to sphere border
    for (unsigned int sampleIndex = 0; sampleIndex <= rad; sampleIndex++)
    {
      // Sample angle equidistantly in width/2 steps.
      // This is only to have a good number of sampling points, it does NOT
      // assume the circle radius to be in the range width/2.
      // Nevertheless sample the range of the given radius.
      double currentTheta = MaxCamAngle_ * double(sampleIndex) / double(rad);

      // pick radius
      radiusInPixel.push_back(PS.EvaluatePolynomial(currentTheta, acCoeff_));
      thetaInRad.push_back(currentTheta);
      // check if we have to increase the radius
      if (radiusInPixel.back() > maxradius)
        maxradius = radiusInPixel.back();
    }
  }
 
  // set radius of outer image rim
  radius_ = maxradius;

  // number of steps for azimuth angle sampling
  const int steps = 14;

  // numerically deriving distortion function in 0
  double thetaPerPixelCenter = thetaInRad[1] - thetaInRad[0];
  Interpolator interpolatorThetaToPixels;
  interpolatorThetaToPixels.SetKnotPoints(thetaInRad);
  interpolatorThetaToPixels.SetControlPoints(radiusInPixel);
  interpolatorThetaToPixels.InitSpline();

  //MaxCamAngle_ = thetaInRad.back();
  for (double sampleTheta = 0; sampleTheta - 1e-10 <= MaxCamAngle_;
       sampleTheta += MaxCamAngle_/steps)
  {
    ang_opt = sampleTheta;

    interpolatorThetaToPixels.Spline(pix_def, ang_opt);

    ang_def = pix_def * thetaPerPixelCenter;
    distAngles.push_back(ang_opt);
    undistAngles.push_back(ang_def);
  }

  return SetUndistortion(distAngles, undistAngles, radius_);
}


void ProjectionParametersSpherical::
TransfCoordImage2Sphere_ (const HomgPoint2D& Source,
                          double &theta, double &phi) const
{
  HomgPoint2D SourceH = Source;
  SourceH.Homogenize();

  double rSrc = sqrt((SourceH[0]-principalX_)*(SourceH[0]-principalX_)+
                     (SourceH[1]-principalY_)*(SourceH[1]-principalY_));
  phi = atan2(SourceH[1]-principalY_,SourceH[0]-principalX_);

  if (interpolatorInit_) {
    double CalMaxCamAngle = 0;
    // calculate calibrated maximum angle
    const_cast<BIAS::Interpolator*>
      (&interpolatorAngle_)->Spline(CalMaxCamAngle, MaxCamAngle_, 4);
    //interpolatorAngle_.SplineFromLuT(CalMaxCamAngle, MaxCamAngle_);

    // calculate calibrated maximum radius
    double RadiusCalMax = MaxCamAngle_ / CalMaxCamAngle * radius_;
    // calculate calibrated theta and inverse
    double CalTheta = rSrc /RadiusCalMax * MaxCamAngle_;
    const_cast<BIAS::Interpolator*>
      (&interpolatorAngleBack_)->Spline(theta, CalTheta, 4);
    //interpolatorAngleBack_.SplineFromLuT(theta, CalTheta);
  } else {
    theta = rSrc / radius_ * MaxCamAngle_;
  }
}


int ProjectionParametersSpherical::
TransfCoordRotateSphere2Image_(double theta, double phi,
                               double viewCenterX,
                               double viewCenterY,
                               HomgPoint2D &Source) const
{
  double viewTheta, viewPhi;
  HomgPoint2D viewCenter(viewCenterX,viewCenterY,1);
  TransfCoordImage2Sphere_(viewCenter, viewTheta, viewPhi);
  // avoid z-rotation by changing the rotation axes
  Vector3<double> vecPoint(1.0,phi,theta);
  Vector3<double> vecView(1.0,viewPhi,viewTheta);
  //Vector3<double> vecCenter(0,0,1);
  Vector3<double> vecAxis(cos(viewPhi+M_PI/2),sin(viewPhi+M_PI/2),0);
  Vector3<double> vecRot(0,0,0);
  vecPoint = vecPoint.CoordSphereToEuclidean();
  vecView = vecView.CoordSphereToEuclidean();
  /*
  // With 1 rotation and moving axis
  RMatrix rMat;
    rMat.Set(vecAxis, viewTheta);
  double x,y,z;
  rMat.GetRotationAnglesZYX(x,y,z);
  vecRot = rMat*vecPoint;
  //cout << x << "," << y << "," << z << endl;
  */

  //With Quaternions
  Quaternion<double> rot;
  rot.SetValueAsAxisRad (vecAxis, viewTheta);
  rot.MultVec(vecPoint, vecRot);

  /*
  //With 2 rotations alpha, beta (wrong rotation!!)
  // viewAlpha: angle between x-z-Plane and vecView
  double viewAlpha = -1 * atan2(vecView[1],vecView[2]);
  // viewBeta: angle between y-z-Plane and vecView
  double viewBeta = atan2(vecView[0],vecView[2]);
  RMatrix rMat;
  rMat.SetZYX(viewAlpha,viewBeta,0);
  vecRot = rMat*vecPoint;
  */

  vecRot = vecRot.CoordEuclideanToSphere();
  phi = vecRot[1];
  theta = vecRot[2];

  if (theta > MaxCamAngle_) return -1;
  if (phi>M_PI) phi-=2*M_PI;

  double rSrc = 0;
  if (interpolatorInit_) {
    double CalMaxCamAngle=0;
    double CalTheta=0;
    // calculate calibrated angles
    interpolatorAngle_.SplineFromLuT(CalMaxCamAngle, MaxCamAngle_);
    interpolatorAngle_.SplineFromLuT(CalTheta, theta);
    // apply calculate calibrated radius from calibrated angles
    rSrc = radius_ * CalTheta / CalMaxCamAngle;
  } else {
    rSrc = radius_ * theta / MaxCamAngle_;
  }

  Source[0] = rSrc * cos(phi) + principalX_;
  Source[1] = rSrc * sin(phi) + principalY_;
  Source[2] = 1;

  return 0;
}


void ProjectionParametersSpherical::
TransfCoordSphere2Image_(double theta, double phi,
                         HomgPoint2D &Source) const
{
  EnforceNormalRange_(theta, phi);

  double rSrc = 0;
  if (interpolatorInit_) {
    double CalMaxCamAngle=0;
    double CalTheta=0;
    // calculate calibrated angles
    interpolatorAngle_.SplineFromLuT(CalMaxCamAngle, MaxCamAngle_);
    interpolatorAngle_.SplineFromLuT(CalTheta, theta);
    // apply calculate calibrated radius from calibrated angles
    rSrc = radius_ * CalTheta / CalMaxCamAngle;
  } else {
    rSrc = radius_ * theta / MaxCamAngle_;
  }

  Source[0] = rSrc * cos(phi) + principalX_;
  Source[1] = rSrc * sin(phi) + principalY_;
  Source[2] = 1;
}


void ProjectionParametersSpherical::
TransfCoordSphere2CalibImage_(double theta, double phi,
                              HomgPoint2D &Source) const
{
  EnforceNormalRange_(theta, phi);

  // apply radius from calibrated angles
  double rSrc = radius_ * theta / MaxCamAngle_;

  Source[0] = rSrc * cos(phi) + principalX_;
  Source[1] = rSrc * sin(phi) + principalY_;
  Source[2] = 1;
}


void ProjectionParametersSpherical::
TransfCoordCalibImage2Sphere_ (const HomgPoint2D& Source,
                               double &theta, double &phi) const
{
  HomgPoint2D SourceH = Source;
  SourceH.Homogenize();

  double rSrc = sqrt((SourceH[0]-principalX_)*(SourceH[0]-principalX_)+
                     (SourceH[1]-principalY_)*(SourceH[1]-principalY_));
  phi = atan2(SourceH[1]-principalY_,SourceH[0]-principalX_);
  theta = rSrc / radius_ * MaxCamAngle_;
}


void ProjectionParametersSpherical::
EnforceNormalRange_(double& theta, double& phi) const
{
  // theta and phi are periodic in 2*M_PI, if they are out of a suitable
  // range, try to find best equivalent angle by adding 2*l*M_PI

  // make sure phi and theta dont contain illegal numbers
  INFNANCHECK(theta);
  INFNANCHECK(phi);

  // the following range adaption performs only if theta and phi are close to [0;2pi[
  // what they ALWAYS should be, check it just for safety:
  BIASASSERT(fabs(theta)<20.0);
  BIASASSERT(fabs(phi)<20.0);

  // bring theta to range [0;2pi[
  while (theta >= M_PI*2.0) theta -= M_PI*2.0;
  while (theta <  0.0)      theta += M_PI*2.0;

  // bring phi to range [-pi;pi[
  while (phi >= M_PI) phi -= M_PI*2.0;
  while (phi < -M_PI) phi += M_PI*2.0;
}


double ProjectionParametersSpherical::
ViewDifference(const ProjectionParametersBase* pPPB) const
{
  const ProjectionParametersSpherical *pPPS =
     dynamic_cast<const ProjectionParametersSpherical* >(pPPB);
  // different projection model has a very different view
  if (!pPPS) {
    BIASERR("View difference for different projection models requested !");
    return DBL_MAX;
  }

  Vector3<double> C0 = pPPS->GetC();
  // Optical axes of the 2 cameras
  Vector3<double>  A0, A;

  RMatrix R = GetR(), R0 = pPPS->GetR();

  A[0] = R[0][2]; A[1] = R[1][2]; A[2] = R[2][2];
  A.Normalize();

  A0[0] = R0[0][2]; A0[1] = R0[1][2]; A0[2] = R0[2][2];
  A0.Normalize();

  // compute field of view, assuming const K !
  double coshalffieldofview = cos(MaxCamAngle_);

  double newness = (GetC()-C0).NormL2();
  newness += 1.0/(1.0-coshalffieldofview)*(1.0-fabs(A.ScalarProduct(A0)));
  return newness;
}


bool ProjectionParametersSpherical::
GetPerspectiveCutOutParameters(const HomgPoint2D& viewCenter,
                               const double perspHalfViewingAngleDEG,
                               ProjectionParametersPerspective& pp) const
{
  double halffov = perspHalfViewingAngleDEG * M_PI/180.0;

  // calculate rotation quaternion
  double viewTheta, viewPhi;
  TransfCoordImage2Sphere_(viewCenter, viewTheta, viewPhi);
  Vector3<double> vecAxis(cos(viewPhi+M_PI/2), sin(viewPhi+M_PI/2), 0);
  Quaternion<double> rot;
  rot.SetValueAsAxisRad (vecAxis, viewTheta);

  // measure 1 deg angle in pixel at viewCenter
  vector<Vector3<double> > vecRPT;//unit sphere points spanning 1Deg theta area
  vector<HomgPoint2D> vecSpProj; //projected into image
  vecRPT.push_back(Vector3<double>(1.0, -M_PI,   0.5/180.0*M_PI));
  vecRPT.push_back(Vector3<double>(1.0, 0,       0.5/180.0*M_PI));
  vecRPT.push_back(Vector3<double>(1.0, -M_PI/2, 0.5/180.0*M_PI));
  vecRPT.push_back(Vector3<double>(1.0, M_PI/2,  0.5/180.0*M_PI));
  for (unsigned int i=0; i<vecRPT.size(); i++) {
    Vector3<double>vecRot;
    rot.MultVec(vecRPT[i].CoordSphereToEuclidean(), vecRot);
    vecSpProj.push_back(ProjectLocal(vecRot));
  }
  double MaxPixDiff=0;
  for  (unsigned int i=0; i<vecSpProj.size(); i+=2) {
    if ((vecSpProj[i]-vecSpProj[i+1]).NormL2()>MaxPixDiff) {
      MaxPixDiff = (vecSpProj[i]-vecSpProj[i+1]).NormL2();
    }
  }
  int imgsize = int(MaxPixDiff*tan(halffov)/tan(M_PI/180.0));
  double focallength = imgsize/tan(halffov);

  // set ProjectionParametersPerspective
  pp.SetQC(GetPose().GetQ()*rot, GetC());
  pp.SetCov(GetCov());
  pp.SetImageSize(imgsize*2,imgsize*2);
  pp.SetFocalLengthAndAspect(focallength, 1);
  pp.SetPrincipal(imgsize,imgsize);
  pp.SetUndistortion(0,0,0,0);

  return true;
}


KMatrix ProjectionParametersSpherical::
GetFakeKMatrix(double& imgsize, int resolution, const double& maxangle) const
{
  imgsize = 0;
  // try user specified maxangle
  double ang = maxangle;
  // check if camera supports such large angles, otherwise clip
  if (ang > GetMaxCamAngle()) ang = GetMaxCamAngle() - 0.01;
  return ProjectionParametersBase::GetFakeKMatrix(imgsize, resolution, ang);
}


#ifdef BIAS_HAVE_XML2

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


int ProjectionParametersSpherical::XMLOut(const xmlNodePtr Node,
                                            XMLIO& XMLObject) const
{
  xmlNodePtr theNode;
  string TopLevelName;
  double Version;
  ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);
  theNode = XMLObject.addChildNode(Node, TopLevelName);
  XMLObject.addAttribute(theNode, "Version", Version);
  ProjectionParametersBase::XMLOut(theNode, XMLObject);

  xmlNodePtr childNode, childNode2;
  XMLObject.addAttribute(Node,"Radius", radius_);
  XMLObject.addAttribute(Node,"MaxAngle", MaxCamAngle_);
  if (interpolatorInit_) {
    childNode = XMLObject.addChildNode(Node,"Distortion");
    vector<double> controlpoints, knotpoints;
    interpolatorAngle_.GetControlPoints(controlpoints);
    interpolatorAngle_.GetKnotPoints(knotpoints);
    for (unsigned int i=0; i<controlpoints.size(); i++) {
      childNode2= XMLObject.addChildNode(childNode,"MeasPoint");
      XMLObject.addAttribute(childNode2, "AngRay", knotpoints[i]);
      XMLObject.addAttribute(childNode2, "AngDeformed", controlpoints[i]);
    }
  }
  return 0;
}


int ProjectionParametersSpherical::XMLIn(const xmlNodePtr Node,
                                           XMLIO& XMLObject)
{
  string TopLevelName;
  double Version;
  xmlNodePtr childNode;

  ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);
  if ((childNode = XMLObject.getChild(Node, TopLevelName))==NULL) {
    BIASERR("Error in xml, no tag" << TopLevelName);
    return -1;
  }
  if (ProjectionParametersBase::XMLIn(childNode, XMLObject)!=0) return -1;

  radius_ = XMLObject.getAttributeValueDouble(Node, "Radius");
  xmlAttrPtr maxAngleAttr = XMLObject.getAttributeByName(Node, "MaxAngle");
  if (maxAngleAttr)
    MaxCamAngle_ = XMLObject.getAttributeValueDouble(maxAngleAttr);

  // Read distortion, if given, either as control points from spline interpolation
  // or as the values of the polynom
  childNode = XMLObject.getChild(Node, "Distortion");
  if (childNode)
  {
    xmlNodePtr childNode2 = XMLObject.getFirstChild(childNode);
    string nodeName2 = (childNode2 ? XMLObject.getNodeName(childNode2) : "");
    if (nodeName2.compare("MeasPoint") == 0)
    {
      // here we have the control points representation
      vector<double> AngleCorrX, AngleCorrY;
      while (childNode2) {
        nodeName2 = XMLObject.getNodeName(childNode2);
        if (nodeName2.compare("MeasPoint") == 0) {
          double x = XMLObject.getAttributeValueDouble(childNode2, "AngRay");
          double y = XMLObject.getAttributeValueDouble(childNode2, "AngDeformed");
          AngleCorrX.push_back(x);
          AngleCorrY.push_back(y);
        }
        childNode2 = XMLObject.getNextChild(childNode2);
      }
      SetUndistortion(AngleCorrX, AngleCorrY, radius_);
    }
    else
    {
      // this is the spline interpolation representation
      childNode2 = XMLObject.getChild(childNode, "AngleCalibPoly");
      if (childNode2) {
        MaxCamAngle_ = XMLObject.getAttributeValueDouble(childNode2, "MaxCamAngle");
        vector<double> coefficients;
        coefficients.push_back(XMLObject.getAttributeValueDouble(childNode2, "C0"));
        coefficients.push_back(XMLObject.getAttributeValueDouble(childNode2, "C1"));
        coefficients.push_back(XMLObject.getAttributeValueDouble(childNode2, "C2"));
        coefficients.push_back(XMLObject.getAttributeValueDouble(childNode2, "C3"));
        coefficients.push_back(XMLObject.getAttributeValueDouble(childNode2, "C4"));
        InitAngleCorrFromPoly(MaxCamAngle_, coefficients);
      } else {
        BIASERR("No polynomial distortion parameters given (either XML nodes MeasPoint or "
                "XML node AngleCalibPoly as child of XML node Distortion)!");
      }
    }
  } else {
    // No distortion is given, hence max. angle must be set as attribute!
    interpolatorInit_ = false;
    if (!maxAngleAttr) {
      BIASERR("Max. camera angle is not set (either use XML attribut MaxAngle or set "
              "distortion parameters)!");
    }
  }
  return 0;
}
#endif


// LM error function
int PolynomialWithoutLinearMonomialError(void *p,int m, int n,const double *x,
                                          double *fvec, int iflag)
{
  vector<double> params;
  ProjectionParametersSpherical *pps = (ProjectionParametersSpherical*)p;

  params.push_back(x[0]);
  params.push_back(0);
  for (int i=1; i<n; i++) {
    params.push_back(x[i]);
  }
  PolynomialSolve PS;
  BIASASSERT(int(pps->myvaluesPolynomialWithoutLinearMonomialError.size())==m);
  BIASASSERT(int(pps->mylocationsPolynomialWithoutLinearMonomialError.size())==m);
  for (int j=0; j<m; j++) {
    fvec[j] = pps->myvaluesPolynomialWithoutLinearMonomialError[j] -
      PS.EvaluatePolynomial(pps->mylocationsPolynomialWithoutLinearMonomialError[j],
                            params);
  }
  return 0;
}

// helper function for EstimateUndistortionPolynomial
int ProjectionParametersSpherical::
FitPolynomialWithoutLinearMonomial_(const unsigned int degree,
                                                                    const std::vector<double>& x,
                                                                    const std::vector<double>& y,
                                                                    std::vector<double> &coefficients)
{
  // See PolynomialSolve::FitPolynomial, but without linear component
  // (see scaramuzza paper)
  BIASASSERT(x.size()==y.size());
  coefficients.resize(degree+1, 0.0);
  Matrix<double> A(x.size(), degree+1);
  Vector<double> B(y.size()), X(degree);
  for (unsigned int row=0; row<x.size(); row++) {
    const double & xc = x[row];
    for (unsigned int col=0; col<degree+1; col++) {
      if (col==1) col=2;
      A[row][col] = 1;
      for (unsigned int colpower=0; colpower<col; colpower++)
        A[row][col] *= xc;
    }
    B[row] = y[row];
  }
  Matrix<double> A2(x.size(), degree);
  for (unsigned int row=0; row<x.size(); row++) {
    A2[row][0] = A[row][0];
    for (unsigned int col=2; col<degree+1; col++) {
      A2[row][col-1] = A[row][col];
    }
  }

  SVD mySVD;
  int result = mySVD.Solve(A2, B, X);


  Vector<double> coeff(degree), resultcoeff(degree);
  coeff[0] = coefficients[0] = X[0];
  coefficients[1] = 0;
  for (unsigned int col=2; col<degree+1; col++) {
    coeff[col-1] = coefficients[col] = X[col-1];
  }

  //these are now class members
  myvaluesPolynomialWithoutLinearMonomialError = y;
  mylocationsPolynomialWithoutLinearMonomialError = x;

  int res;
  if ((res = LevenbergMarquardt(&PolynomialWithoutLinearMonomialError, NULL,
                                x.size(),degree,
                                coeff,resultcoeff,
                                MINPACK_DEF_TOLERANCE))!=0) {
     BIASERR("Error in Levenberg-Marquardt-Optimization of F: "<<res);
     //return -1;
  }

  coefficients[0] = resultcoeff[0];
  coefficients[1] = 0;
  for (unsigned int col=2; col<degree+1; col++) {
    coefficients[col] = resultcoeff[col-1];
  }

  return result;
}

int ProjectionParametersSpherical::
EstimateUndistortionPolynomial(std::vector<double>& coefficients,
                               const unsigned int degree,
                               bool LinearMonomialIsZero)
{
  vector<double> a,b;
  a.reserve((unsigned int)(radius_+1)); b.reserve((unsigned int)(radius_+1));
  // run from principal point to fov circle
  for (unsigned int i=1; i<radius_; i++) {
    // compute 2D point in image
   HomgPoint2D pos(principalX_+double(i),principalY_,1);
   Vector3<double> rPhiTheta(1,0,0);
   TransfCoordImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);
   // compute ray
   Vector3<double> ret = rPhiTheta.CoordSphereToEuclidean();
   //scale the first two components to have the length of the radius, 
   //than I will have the appropriate length of the z-component and can
   //make the assumption that the optical ray at this radius and 
   //ret are parallel but have different length. Why the negative sign?
   //probably the combination of spherical to euclidean transformations.
   ret *= -1.0 * double(i) / sqrt((ret[0]*ret[0] + ret[1]*ret[1]));
   // save radius
   a.push_back(double(i));
   // versus z-coordinate
   b.push_back(ret[2]);
  }
  if (LinearMonomialIsZero) {
    return FitPolynomialWithoutLinearMonomial_(degree, a, b, coefficients);
  } else {
    PolynomialSolve PS;
    return PS.FitPolynomial(degree, a, b, coefficients);
  }
}

int ProjectionParametersSpherical::
EstimateDistortionPolynomial(std::vector<double>& coefficients,
                             const unsigned int degree,
                             bool LinearMonomialIsZero)
{
  vector<double> a,b;
  a.reserve((unsigned int)(radius_+1));
  b.reserve((unsigned int)(radius_+1));

  double CalMaxCamAngle=0;
  interpolatorAngle_.SplineFromLuT(CalMaxCamAngle, MaxCamAngle_);
  // run from principal point to fov circle
  a.push_back(0);
  b.push_back(0);
  for (unsigned int i=1; i<radius_; i++) {
    // sample angle equidistantly in radius_ steps
    double theta = MaxCamAngle_ * double(i)/double(radius_);
    double CalTheta=0;
    // calculate calibrated angles
    interpolatorAngle_.SplineFromLuT(CalTheta, theta);
    // apply calculate calibrated radius from calibrated angles
    double rSrc = radius_ * CalTheta/CalMaxCamAngle;
    // save theta
    a.push_back(theta);
    // versus radius
    b.push_back(rSrc);
  }
  if (LinearMonomialIsZero) {
    return FitPolynomialWithoutLinearMonomial_(degree, a, b, coefficients);
  } else {
    PolynomialSolve PS;
    return PS.FitPolynomial(degree, a, b, coefficients);
  }
}

void ProjectionParametersSpherical::
ProjectOutsidePositions_(int& posX, int& posY) 
{
  if(posX<0) posX=0;
  if(posY<0) posY=0;
  if(posX>static_cast<int>(width_-1)) posX=width_-1;
  if(posY>static_cast<int>(height_-1)) posY=height_-1;
}

void ProjectionParametersSpherical::GetFirstBorderPixel(PixelIterator& it) {
  //as no double bresenham circle implementation exists yet, we reduce the radius 
  int rad = (int)rint(floor(radius_));
  int center[2] = {(int)rint(principalX_), (int)rint(principalY_)};
  fovCircle_.Init(center,rad);
  int next[2];
  fovCircle_.GetNext(next);
  ProjectOutsidePositions_(next[0], next[1]);
  it.x = next[0];
  it.y = next[1];

}

bool ProjectionParametersSpherical::GetNextBorderPixel(PixelIterator& it) {
  int next[2];
  bool res = fovCircle_.GetNext(next);
  ProjectOutsidePositions_(next[0], next[1]);
  it.x = next[0];
  it.y = next[1];
  return res;
}

// void ProjectionParametersSpherical::GetFirstEdgePosition(PixelIterator& it) {
//   it.x = -0.5;
//   it.y = -0.5;
// }

// bool ProjectionParametersShperical::GetNextEdgePosition(PixelIterator& it) {
//   if (it.x==-0.5) {
//     if (it.y==-0.5) {
//       // upper left image pixel, move right
//       it.x++;
//       return true;
//     }
//     // on way back up at left image border
//     it.y--;
//     // return false if finished
//     return (it.y>-0.5);
//   } else if (it.x<width_-0.5) {
//     if (it.y==-0.5) {
//       // moving left on upper image boundary
//       it.x++;
//       return true;
//     }
//     // moving right on lower image boundary
//     it.x--;
//     return true;
//   } else {
//     if (it.y<height_-0.5) {
//       // moving down on right image side
//       it.y++;return true;
//     }
//     // lower right corner
//     it.x--;
//     return true;
//   }
// }