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


/**
   @file TestProjectionParametersSphericalSimpleFast.cpp
   @brief Test the spherical projection parameters
   @relates ProjectionParametersSphericalFast
   @ingroup g_tests
   @author MIP
*/


#include <Geometry/ProjectionParametersSphericalFast.hh>
#include <Base/Common/SharedPtr.hh>
#include <Base/Math/Random.hh>

using namespace BIAS;
using namespace std;

int main(int argc, char *argv[]) {
  
  // create the proj param object
  const unsigned width = 640, height = 480;
  ProjectionParametersSphericalFast* proj = 
     new ProjectionParametersSphericalFast(600, 75.0/180.0*M_PI, width, height);
  
  RMatrixBase Rtmp;
  Rtmp.SetXYZ(1.0, 2.15, -0.7);
  const RMatrixBase R(Rtmp); // ensure const R
  proj->SetR(R);
  const Vector3<double> C(13.0, 24.415, -4.17);
  proj->SetC(C);

  vector<double> coeffsDist(5, 0.0);
  coeffsDist[0] = 1.01;
  coeffsDist[1] = -0.025;
  coeffsDist[2] = 0.02;
  proj->InitPolyCoeffs(75.0/180.0*M_PI, coeffsDist, false);
  
  proj->TestPolynomialInversion();
  
  
  cout << "projection " << *proj << endl;
  
  // test clone and operator=
  cout << "testing clone and = operator " << endl;
  ProjectionParametersSphericalFast* proj2 = 
    (dynamic_cast<ProjectionParametersSphericalFast*>(proj->Clone()));
  if(proj->DoIntrinsicsDiffer(proj2)){
    BIASERR("clone operator does not work" << endl);
    return -1;
  }
  cout << "clone " << *proj2 << endl;
  ProjectionParametersSphericalFast proj3 = *proj;
  if(proj->DoIntrinsicsDiffer(&proj3)){
    BIASERR("= operator does not work" << endl);
    return -1;
  }
  cout << "= " << proj3 << endl;
  

#ifdef BIAS_HAVE_XML2
  // test io
 
  const string filename = "TestProjectionParametersSimple.xml";
  if (proj->XMLWrite(filename)!=0) { BIASABORT; }
  ProjectionParametersSphericalFast proj6;
  if (proj6.XMLRead(filename)!=0) { BIASABORT; }
  cout << "proj " << *proj << endl;
  cout << "proj6 " << proj6 << endl;
  if(proj->DoIntrinsicsDiffer(&proj6)){
    BIASERR("xml input/output does not work" << endl);
    return -1;
  }

  string tmp;
  if (proj->XMLWriteToString(tmp)!=0) { BIASABORT; }
  ProjectionParametersSphericalFast proj5;
  if (proj5.XMLReadFromString(tmp)!=0) { BIASABORT; }
  cout << "proj " << *proj << endl;
  cout << "proj5 " << proj5 << endl;
  if(proj->DoIntrinsicsDiffer(&proj5)){
    BIASERR("xml streaming does not work" << endl);
    return -1;
  }
#endif
  
  cout << "proj used for project-unproject test " << *proj << endl;
  proj->TestPolynomialInversion();
  Vector3<double> p3dtmp(-15,-10,100);
  HomgPoint2D p2dtmp = proj->ProjectLocal(p3dtmp);
  cout << "p3d " << p3dtmp << " p2d " << p2dtmp << endl;
  Vector3<double> pos, dir;
  proj->UnProjectLocal(p2dtmp, pos, dir);
  double lambda = (p3dtmp[0]-pos[0])/dir[0];
  double lambda2 = (p3dtmp[1]-pos[1])/dir[1];
  double lambda3 = (p3dtmp[2]-pos[2])/dir[2];
  cout << "lambda " << lambda << " " << lambda2 << " " << lambda3 << endl;
  cout << "unprojected " << pos << " " << dir << " point " << pos + lambda3 * dir << endl;
  
  //BIASABORT;
  
  // test project and unproject consistency
  Vector3<double> p3d, ref, p;
  Random r;
  HomgPoint2D p2d;
  int num;
  const int num_tries = 1e4;
  double minAcc = 1e-4;
  for (int n=0; n<num_tries; n++){
    // generate random 3d point p3d
    num = 0;
    do {
      p3d.Set(r.GetUniformDistributed(-10.0, 10.0), 
              r.GetUniformDistributed(-10.0, 10.0), 
              r.GetUniformDistributed(-10.0, 10.0));
      num++;
    } while (Equal(p3d.NormL2(), 0.) && num<100);
    if (Equal(p3d.NormL2(), 0.)) { BIASABORT; }
    Vector3<double> p3dorig = p3d;
    p3d /= p3d.NormL2();

    // test local project and unproject consistency
    p2d = proj->ProjectLocal(p3d);
 //   cout << "p2d " << p2d << " p3d " << p3d << " p3d orig " << p3dorig << endl;
    if(p2d[0] == 0 && p2d[1] == 0 && p2d[2] == 0){
 //     cout << "point outside image" << endl;
    } else {
      proj->UnProjectLocal(p2d, p, ref);
      if (Equal(ref.NormL2(), 0.)){ BIASABORT; }
      ref /= ref.NormL2(); // scale ref for comparison
      double error = (ref-p3d).NormL2();
      if(error > minAcc){
        cout << "high error orig : " << p3d << " proj/unproj " << ref << " error " << error << endl;
      }
    }

    // test project and unproject consistency
    p2d = proj->Project(HomgPoint3D(p3d));
    if(p2d[0] == 0 && p2d[1] == 0 && p2d[2] == 0){
    //  cout << "point outside image" << endl;
    } else {
      double depth = (p3d-C).NormL2();
      HomgPoint3D refh = proj->UnProjectToImagePlane(p2d, depth);
      ref = refh.GetEuclidean();
      if (Equal(ref.NormL2(), 0.)){ BIASABORT; }
      ref /= ref.NormL2(); // scale ref for comparison
      double error = (ref-p3d).NormL2();
      if(error > minAcc){
        cout << "high error in second part orig " << p3d << " proj/unproj " << ref << " error " << error << endl;
      }
    }

 
  }

  return 0;
}