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

/**
   @example ExampleEstimateFisheyePolynomial.cpp
   @relates PolynomialSolve, ProjectionParametersSpherical
   @brief Example for estimating the polynomial of a fisheye camera
   @ingroup g_examples
   @author MIP
*/

// BIAS
#include <Geometry/ProjectionParametersSpherical.hh>
#include <Geometry/Projection.hh>
#include "Base/Math/Random.hh"
#include "MathAlgo/PolynomialSolve.hh"

using namespace BIAS;
using namespace std;


int main(int argc, char *argv[]) {

  // test distortion or undistortion parametrization
  bool testDistortion = true;
  bool skipLinearMonom = !testDistortion;
  Projection P;
  
  if (argc<2) {
    cout<<"error, no xml file given."<<endl;
    return -2;
  }
  if (P.Load(argv[1])!=0) {
    cout<<"error, could not load xml."<<endl;
    return -2;
  }
 
  ProjectionParametersSpherical *ppS = 
    dynamic_cast<ProjectionParametersSpherical *>(P.GetParameters());
  if (ppS == NULL) {
    cout<<"error, no spherical parameters."<<endl;
    return -2;
  }
  unsigned int width, height;
  ppS->GetImageSize(width,height);
  cout<<"maxcamangle of loaded projection is "<<  ppS->GetMaxCamAngle()<<endl;
 
  int degree = 4;
  std::vector<double> coefficients;
  coefficients.resize(degree+1);

  cout<<endl<<endl<<endl<<"now estimating polynomial"<<endl;
  if (testDistortion) {
    cout<<"distortion function is used ..."<<endl;
    ppS->EstimateDistortionPolynomial(coefficients, degree, skipLinearMonom);
  } else {
    cout<<"undistortion function is used ..."<<endl;
    ppS->EstimateUndistortionPolynomial(coefficients, degree, skipLinearMonom);
  }
  cout<<"coefficients are ";
  for (unsigned int i=0; i<coefficients.size(); i++) {
    cout<<coefficients[i]<<" ";
  }
  cout<<endl;

  // generate new parameters and test if it is the same
  ProjectionParametersSpherical S2;
  S2.SetImageSize(width,height);
  S2.SetRadius(ppS->GetRadius());
  double px=0, py=0;
  ppS->GetPrincipal(px,py);
  S2.SetPrincipal(px,py);
  S2.SetPose(ppS->GetPose());
  //S2 = *ppS;
  

  cout<<endl<<endl<<endl<<"now initing with polynomial"<<endl;
 
  //  S2.InitAngleCorrFromPoly(1.57079633e+00,
  //                            -3.01118447e+02,
  //                            0.00000000,
  //                            1.29706584e-03,
  //                            -1.07072253e-06,
  //                            2.34508181e-09); 

  S2.InitAngleCorrFromPoly(ppS->GetMaxCamAngle(),
                           coefficients,
                           !testDistortion); 


  cout<<endl<<endl<<endl<<"now estimating polynomial again "<<endl;


  if (testDistortion) {
    cout<<"distortion function is used ..."<<endl;
    ppS->EstimateDistortionPolynomial(coefficients, degree, skipLinearMonom);
  } else {
    cout<<"undistortion function is used ..."<<endl;
    ppS->EstimateUndistortionPolynomial(coefficients, degree, skipLinearMonom);
  }
  cout<<"coefficients are ";
 
  for (unsigned int i=0; i<coefficients.size(); i++) {
    cout<<coefficients[i]<<" ";
  }
  cout<<endl;

  cout<<"generating some 3D points with original projection .."<<endl;

  Random R;
  vector<HomgPoint3D> points3D;
  vector<HomgPoint2D> points2D;
  for (unsigned int i=0; i<20; i++) {
    HomgPoint2D x(R.GetUniformDistributed(width/4, width*3/4),
                  R.GetUniformDistributed(width/4, width*3/4));
    HomgPoint3D X = ppS->UnProjectToPoint(x, 1.0);
    if (X.NormL2()>0.1) {
      points2D.push_back(x);
      points3D.push_back(X);
    }
  }


  

  BIAS::Pose thePose(ppS->GetPose());
  PolynomialSolve PS;
  double averageError = 0;
  for (unsigned int i=0; i<points3D.size(); i++) {
    

    Vector3<double> pRay, pos;
    S2.UnProjectLocal(S2.Project(points3D[i]), pos, pRay);
    //cout<<"perfect ray is "<<pRay<<endl;

    //cout<<"3D point is "<<X<<endl;

    // transform into camera coordinate system
    HomgPoint3D XCCS(thePose.GlobalToLocal(points3D[i]));
    Vector3<double> ProjectionRay(XCCS[0], XCCS[1], XCCS[2]);
    ProjectionRay.Normalize();






    if (testDistortion) {

      // we have a distortion polynomial,
      Vector3<double> rPhiTheta = ProjectionRay.CoordEuclideanToSphere();  
      if(Equal(rPhiTheta[0], 0.0)) {
        BIASERR("sphere coordinate transform failed for ray "
                << ProjectionRay);
      }
      
      double radius = PS.EvaluatePolynomial(rPhiTheta[2], coefficients);
      HomgPoint2D result(0,0);
      result[0] = radius * cos(rPhiTheta[1]) + px;
      result[1] = radius * sin(rPhiTheta[1]) + py;
      cout<<"projects to "<<result<<", measured at "<<points2D[i]
          <<" diff is "<<(result-points2D[i]).NormL2()<<endl;

      averageError += (result-points2D[i]).NormL2();

      result = S2.Project(points3D[i]);
      cout<<"Reconstructed projects to "<<result<<", measured at "<<points2D[i]
          <<" diff is "<<(result-points2D[i]).NormL2()<<endl;

    } else {
      //cout<<"projection ray is "<<ProjectionRay<<endl;
      Vector3<double> ImageRay(points2D[i][0]-px, 
                               points2D[i][1]-py, 
                               0);
      double radius = sqrt(ImageRay[0]*ImageRay[0] + ImageRay[1]*ImageRay[1]);
      ImageRay[2] = -1.0*PS.EvaluatePolynomial(radius, coefficients);

      //cout<<"image ray is "<<ImageRay<<endl;
      ImageRay.Normalize();
      // cout<<"image ray normalized is "<<ImageRay<<endl;
      if (ImageRay.ScalarProduct(ProjectionRay)<0.0)
        ImageRay *= -1.0;
      cout<<"ray error for manual polynomial projection in degree: ";
      averageError += acos(ImageRay.ScalarProduct(ProjectionRay))*180.0/M_PI;
      cout<<acos(ImageRay.ScalarProduct(ProjectionRay))*180.0/M_PI
          <<" degree"<<endl; 
      cout<<"ray error for reconstructed interpolator projection in degree: ";
      cout<<acos(ImageRay.ScalarProduct(pRay))*180.0/M_PI<<" degree"<<endl;
    }

    Vector3<double> p, dir;
    ppS->UnProjectLocal(points2D[i], p, dir);
    HomgPoint2D
      testpoint1 = ppS->ProjectLocal(dir),
      testpoint2 = points2D[i];
    testpoint1.Normalize();
    testpoint2.Normalize();
    cout<<"ray error (inconsistency) for original project/unproject: ";
    cout<<acos(testpoint1.ScalarProduct(testpoint2))*180.0/M_PI
        <<" degree"<<endl;


    //for (unsigned int i=0; i<3; i++) {
    //cout<< ImageRay[i] - ProjectionRay[i] <<" ";
    //}
    cout<<endl;
  }
  averageError /= points3D.size();
  cout<<"Average angular error (manual) is "<<averageError <<endl;

  
  return 0;
}