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

using namespace BIAS;
using namespace std;

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


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

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

  const ProjectionParametersSphericalFast* pps =
    dynamic_cast<const ProjectionParametersSphericalFast*>(p);
  if(pps == NULL){
    cout << "casted to null pointer " << endl;
    return true;
  }

  if(!BIAS::Equal(radius_, pps->radius_, 1e-8)){
    cout << "radii do not match " << endl;
    return true;
  }

  if(!BIAS::Equal(maxCamAngle_, pps->maxCamAngle_, 1e-8)){
    cout << "cam angles do not match " << endl;
    return true;
  }

  return false;
}


bool ProjectionParametersSphericalFast::
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 ProjectionParametersSphericalFast::
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;
  }
  
  TransfCoordSphere2Image_(rPhiTheta[2], rPhiTheta[1], p2d);
  return 0;
}


HomgPoint3D ProjectionParametersSphericalFast::
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  ProjectionParametersSphericalFast::
UnProjectLocal(const HomgPoint2D& pos, Vector3<double>& origin,
               Vector3<double>& direction, bool IgnoreDistortion) const {

  Vector3<double> rPhiTheta(1,0,0);
  TransfCoordImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);

  direction = rPhiTheta.CoordSphereToEuclidean();
  direction.Normalize();
  origin = Vector3<double>(0.0);
}


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



void ProjectionParametersSphericalFast::
SetMaxCamAngle(const double maxCamAngle){
  maxCamAngle_ = maxCamAngle;
}



int ProjectionParametersSphericalFast::
InitPolyCoeffs(const double maxCamAngle,
               const std::vector<double>& coefficients,
               const bool undistCoeffs) {
  
  // calculate distortion function
  maxCamAngle_ = maxCamAngle;

  if (undistCoeffs) { // coefficients describe undistortion
    coeffsUndist_ = coefficients;
    // degree is the number of coefficients
    EstimateDistortionPolynomial(coeffsDist_, coeffsUndist_.size());
  } else { // coefficients described distortion
    coeffsDist_ = coefficients;
    // degree is the number of coefficients
    EstimateUndistortionPolynomial(coeffsUndist_, coeffsDist_.size());
  }
 
  
  return 0;
}


void ProjectionParametersSphericalFast::TestPolynomialInversion(){
  
  double avgError = 0;
  double error = 0;
  double val = 0;
  double valInv = 0;
  unsigned int counter = 0;
  for(double a=0.0; a < maxCamAngle_; a+=0.1){
    val = EvaluatePolynomial(a, coeffsDist_);
    valInv = EvaluatePolynomial(val, coeffsUndist_);
    error = fabs(a-valInv);
    avgError += error;
    counter++;
  }
   
  cout << "TestPolynomial Inversion: average error " << error/(double)counter << endl;
  
}


void ProjectionParametersSphericalFast::
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_);
  
  double calMaxCamAngle = EvaluatePolynomial(maxCamAngle_, coeffsDist_);
  
  // calculate calibrated maximum radius
  double RadiusCalMax = maxCamAngle_ / calMaxCamAngle * radius_;
  // calculate calibrated theta and inverse
  double calTheta = rSrc /RadiusCalMax * maxCamAngle_;
  theta = EvaluatePolynomial(calTheta, coeffsUndist_);
}




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

  if(theta > maxCamAngle_){
    // point is outside of image, return zero point
    Source.SetZero();
    return;
  }
  
  double rSrc = 0;
  // calculate calibrated angles
  double calMaxCamAngle = EvaluatePolynomial(maxCamAngle_, coeffsDist_);
  double calTheta = EvaluatePolynomial(theta, coeffsDist_);

  // apply calculate calibrated radius from calibrated angles
  rSrc = radius_ * calTheta / calMaxCamAngle;

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

void ProjectionParametersSphericalFast::
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 ProjectionParametersSphericalFast::
ViewDifference(const ProjectionParametersBase* pPPB) const {
  const ProjectionParametersSphericalFast *pPPS =
     dynamic_cast<const ProjectionParametersSphericalFast* >(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 ProjectionParametersSphericalFast::
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.SetPose(Pose_);
  pp.SetImageSize(imgsize*2,imgsize*2);
  pp.SetFocalLengthAndAspect(focallength, 1);
  pp.SetPrincipal(imgsize,imgsize);
  pp.SetUndistortion(0,0,0,0);

  return true;
}


KMatrix ProjectionParametersSphericalFast::
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 ProjectionParametersSphericalFast::XMLGetClassName(std::string& TopLevelTag,
                                                     double& Version) const {
  TopLevelTag = "ProjectionParametersSphericalFast";
  Version = 0.1;
  return 0;
}


int ProjectionParametersSphericalFast::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_);
  childNode = XMLObject.addChildNode(Node,"Distortion");
  for (unsigned int i=0; i< coeffsDist_.size(); i++) {
    childNode2= XMLObject.addChildNode(childNode,"coeffsDist");
    XMLObject.addAttribute(childNode2, "coeff", coeffsDist_[i]);
  }
  childNode = XMLObject.addChildNode(Node,"Undistortion");
  for (unsigned int i=0; i< coeffsUndist_.size(); i++) {
    childNode2= XMLObject.addChildNode(childNode,"coeffsUndist");
    XMLObject.addAttribute(childNode2, "coeff", coeffsUndist_[i]);
  }
  return 0;
}


int ProjectionParametersSphericalFast::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("coeffsDist") == 0){
      // here we have the coefficients for the distortion polynomial
      coeffsDist_.clear();
      while (childNode2) {
        nodeName2 = XMLObject.getNodeName(childNode2);
        if (nodeName2.compare("coeffsDist") == 0) {
          double c = XMLObject.getAttributeValueDouble(childNode2, "coeff");
          coeffsDist_.push_back(c);
        }
        childNode2 = XMLObject.getNextChild(childNode2);
      }
      InitPolyCoeffs(maxCamAngle_, coeffsDist_, false);
    } 
  }
  return 0;
}
#endif




int InvertPolynomialErrorFunction(void *p,int m, int n, const double *x,
                                  double *fvec, int iflag) {
  
  ProjectionParametersSphericalFast* pps = (ProjectionParametersSphericalFast*)p;
  
  vector<double> params;
  for (int i=0; i<n; i++){
    params.push_back(x[i]);
  }
 
  double error = 0;
  for (int j=0; j<m; j++){
    fvec[j] = pps->myvaluesPolynomial[j] - pps->EvaluatePolynomial(pps->mylocationsPolynomial[j], params);
    error += fvec[j];
  }
//  cout << "error " << error/(double)m << "num params " << n 
//       << " numObs " << m << " size localtions " << pps->mylocationsPolynomial.size()
//       << " values size " << pps->myvaluesPolynomial.size() << endl;
  
  return 0;
}


int ProjectionParametersSphericalFast::FitPolynomial(std::vector<double>& coefficients,
                                                     std::vector<double>& a,
                                                     std::vector<double>& b,
                                                     unsigned int degree){
    
 // cout << "fitting poly linearily " << endl;
  
  coefficients.clear();
  coefficients.resize(degree, 0.0);
  Matrix<double> A(a.size(), degree);
  Vector<double> B(b.size()), X(degree);
  for (unsigned int row=0; row < a.size(); row++) {
    A[row][0] = a[row];
    for (unsigned int col=1; col < degree; col++) {
      A[row][col] = A[row][col-1]*a[row]*a[row];
    }
    B[row] = b[row];
  }
  
 
  SVD mySVD;
  int result = mySVD.Solve(A, B, X);
  if (result!=0) {
    BIASERR("linear solution failed");
    return -1;
  }
//  cout << "linear result " << X << endl;
//  cout << "non-linear fitting " << endl;
  Vector<double> resultcoeff(degree);
  resultcoeff = X;

  // optimize numerically unstable linear result
  myvaluesPolynomial = b;
  mylocationsPolynomial = a;

  if ((result = LevenbergMarquardt(&InvertPolynomialErrorFunction, (void*)(this), a.size(), degree,
                                   X,resultcoeff, 
                                   MINPACK_DEF_TOLERANCE))!=0) {
     BIASERR("Error in Levenberg-Marquardt-Optimization of Polynomial: "<< result);
     // use linear solution
     for (unsigned int col=0; col<degree; col++) coefficients[col] = X[col];
  } else {
    // use these coefficients
    for (unsigned int col=0; col<degree; col++) {
      coefficients[col] = resultcoeff[col];
    }
  }
  
 // cout << "nonlinear result " << resultcoeff << endl;
  
  
  // optimization falied, but linear ok:
  if (result!=0) return 1;

  return 0;
  
}


int ProjectionParametersSphericalFast::
EstimateUndistortionPolynomial(std::vector<double>& coefficients,
                               const unsigned int degree) {

 // cout << "estimating undistortion poly" << endl;
  
  double stepSize = 1.0/180*M_PI;
  unsigned int numSamples = (unsigned int)(maxCamAngle_/stepSize+1);
  vector<double> a(numSamples), b(numSamples);
  
 // cout << "num samples " << numSamples << endl;
  
  unsigned int counter=0;
  for(double theta=0.0; theta < maxCamAngle_ && counter < numSamples; theta+=stepSize, counter++){
    double calTheta = EvaluatePolynomial(theta, coeffsDist_);
    a[counter] = calTheta;
    b[counter] = theta;
  //  cout << "counter " << counter << " a " << a[counter] << " b " << b[counter] << endl;
  }
//  cout << "done sampling " << endl;
  
  return FitPolynomial(coefficients, a, b, degree);
 
}

int ProjectionParametersSphericalFast::
EstimateDistortionPolynomial(std::vector<double>& coefficients,
                             const unsigned int degree) {
  
  //cout << "estimating distortion poly" << endl;

  double calTheta = 0;
  double stepSize = 1.0/180*M_PI;
  double theta = 0;
  // int case of weird polynomials, maxCamAngle may never be reached, so make
  // sure the while loop does not create a memory leak
  unsigned int counter = 0;
  unsigned int highBoundary = (unsigned int)(maxCamAngle_/stepSize*2);

  vector<double> a,b;
  a.reserve(highBoundary);
  b.reserve(highBoundary);
    
  while(calTheta <= maxCamAngle_ && counter < highBoundary){
    calTheta = EvaluatePolynomial(theta, coeffsUndist_);
    a.push_back(theta);
    b.push_back(calTheta);
    theta += stepSize;
    counter++;
  }
  
 // cout << "done sampling " << endl;
  
  return FitPolynomial(coefficients, a, b, degree);
  
  
}

void ProjectionParametersSphericalFast::
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 ProjectionParametersSphericalFast::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 ProjectionParametersSphericalFast::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;
}