TestProjectionParametersSphericalSimple.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 TestProjectionParametersSphericalSimple.cpp
   @brief Test the spherical projection parameters
   @relates ProjectionParametersSphericalSimple
   @ingroup g_tests
   @author MIP
*/


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

using namespace BIAS;
using namespace std;

void TestContent(const ProjectionParametersSphericalSimple *proj,
                 const double phiOffs, const double thetaOffs, 
                 const double dPhi, const double dTheta, const unsigned width,
                 const unsigned height, const RMatrixBase& R,
                 const Vector3<double> &C)
{
  unsigned pw, ph;
  if (proj->GetImageSize(pw, ph)!=0) { BIASABORT; }
  if (pw!=width) { BIASABORT; }
  if (ph!=height) { BIASABORT; }

  // phi and theta are cyclic, it is only relevant, that the loca coos
  // from phi offset and theta offset are identical
  Vector3<double> l1, l2;
  proj->Angles2Local(proj->GetPhiOffset(), proj->GetThetaOffset(), l1);
  proj->Angles2Local(phiOffs, thetaOffs, l2);
  for (int i=0; i<3; i++){
    if (!Equal(l1[i]-l2[i], 0., 1e-9)) { 
      cout << l1<<"\n"<<l2<<"\n"<<l1-l2<<"\n"<<(l1-l2).NormL2()<<endl; 
      BIASABORT; 
    }
  }
  if (!Equal(proj->GetDPhi(), dPhi)) { BIASABORT; }
  if (!Equal(proj->GetDTheta(), dTheta)) { BIASABORT; }

  RMatrixBase Rp = proj->GetRCache(), id = Rp.Transpose() * R;
  if (!id.IsIdentity(1e-10)) { 
    cout << "R: "<<R<<"\nRp: "<<Rp<<"\nid: "<<id<<endl; BIASABORT; 
  }
  Rp = proj->GetR();
  id = Rp.Transpose() * R;
  if (!id.IsIdentity(1e-10)) { 
    cout << "R: "<<R<<"\nRp: "<<Rp<<"\nid: "<<id<<endl; BIASABORT; 
  }
  Vector3<double> Cp = proj->GetC();
  if (!Equal((Cp-C).NormL2(), 0., 1e-13)){
    cout << "C: "<<C<<"\nCp: "<<Cp<<"\tdiff: "<< C-Cp<<endl;
    BIASABORT;
  }
}


int main(int argc, char *argv[])
{
  // create the proj param object
  const double phiOffs = 0.7, thetaOffs = -0.3, dPhi = 0.05, dTheta = 0.025;
  const unsigned width = 640, height = 480;
  SharedPtr<ProjectionParametersSphericalSimple> proj = 
    SharedPtr<ProjectionParametersSphericalSimple>
    (new ProjectionParametersSphericalSimple(phiOffs, thetaOffs, dPhi, dTheta,
                                             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);

  // test clone and operator==
  SharedPtr<ProjectionParametersSphericalSimple> proj2 = 
    SharedPtr<ProjectionParametersSphericalSimple>
    (dynamic_cast<ProjectionParametersSphericalSimple *>(proj->Clone()));
  TestContent(Get(proj2), phiOffs, thetaOffs, dPhi, dTheta, width, height, R, C);
  ProjectionParametersSphericalSimple proj3 = *proj;
  TestContent(&proj3, phiOffs, thetaOffs, dPhi, dTheta, width, height, R, C);
  ProjectionParametersSphericalSimple proj4(*proj);
  TestContent(&proj4, phiOffs, thetaOffs, dPhi, dTheta, width, height, R, C);

#ifdef BIAS_HAVE_XML
  // test io
  string tmp;
  if (proj->XMLWriteToString(tmp)!=0) { BIASABORT; }
  ProjectionParametersSphericalSimple proj5;
  if (proj5.XMLReadFromString(tmp)!=0) { BIASABORT; }
  proj->Print();
  proj5.Print();
  TestContent(&proj5, phiOffs, thetaOffs, dPhi, dTheta, width, height, R, C);
  const string filename = "TestProjectionParametersSimple.xml";
  if (proj->XMLWrite(filename)!=0) { BIASABORT; }
  ProjectionParametersSphericalSimple proj6;
  if (proj6.XMLRead(filename)!=0) { BIASABORT; }
  TestContent(&proj6, phiOffs, thetaOffs, dPhi, dTheta, width, height, R, C);
#endif
 
  // test project and unproject consistency
  Vector3<double> p3d, ref, p;
  Random r;
  Vector2<double> pixel;
  HomgPoint2D p2d;
  int num;
  const int num_tries = 100000;
  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; }
    p3d /= p3d.NormL2();

    // test local project and unproject consistency
    p2d = proj->ProjectLocal(p3d);
    proj->UnProjectLocal(p2d, p, ref);
    if (Equal(ref.NormL2(), 0.)){ BIASABORT; }
    ref /= ref.NormL2(); // scale ref for comparison
    for (int i=0; i<3; i++){
      if (!Equal(ref[i]-p3d[i], 0., 1e-13)) { 
        cout << "orig: "<<p3d<<"\nproj/unproj: "<<ref<<endl
             <<i<<"\t"<<ref[i]-p3d[i]<<endl;
        BIASABORT; 
      }
    }

    // test project and unproject consistency
    p2d = proj->Project(HomgPoint3D(p3d));
    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
    for (int i=0; i<3; i++){
      if (!Equal(ref[i]-p3d[i], 0., 1e-13)) { 
        cout <<   "orig[i]    : "<<setprecision(20)<<p3d[i]
             << "\nproj/unproj: "<<setprecision(20)<<ref[i]<<endl;
        cout <<   "orig       : "<<p3d
             << "\nproj/unproj: "<<ref<<endl
             <<i<<"\t"<<ref[i]-p3d[i]<<endl;
        BIASABORT; 
      }
    }

    // semilocal
    proj->GlobalOrientation2Pixel(p3d, pixel);
    proj->Pixel2GlobalOrientation(pixel, ref);
    if (Equal(ref.NormL2(), 0.)){ BIASABORT; }
    ref /= ref.NormL2(); // scale ref for comparison
    for (int i=0; i<3; i++){
      if (!Equal(ref[i]-p3d[i], 0., 1e-13)) { 
        cout << "orig: "<<p3d<<"\nproj: "<<ref<<endl
             <<i<<"\t"<<ref[i]-p3d[i]<<endl;
        BIASABORT; 
      }
    }
  }

  return 0;
}