TestTriangulation.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 TestTriangulation.cpp
   @brief unit test for Triangulation::TriangulateProjective 
   @relates Triangulation
   @ingroup g_tests
   @author woelk 06/2008 (c) www.vision-n.de 
*/

#include <Geometry/Triangulation.hh>
#include <Geometry/ProjectionParametersPerspective.hh>
#include <Geometry/ProjectionParametersSphericalSimple.hh>
#include <Base/Common/SharedPtr.hh>

using namespace BIAS;
using namespace std;

bool verbose = true;

/**\cond HIDDEN_SYMBOLS */
struct STestData 
{
  SharedPtr<const ProjectionParametersBase> proj1, proj2;
  HomgPoint2D p1, p2;
  HomgPoint3D p3d;
  int triangulation_result;
};
/** \endcond */

void FillTestCases(vector<struct STestData> &cases);

bool RunAndCheck(const struct STestData &data);

  
int main(int argc, char *argv[])
{
  if (argc==2){
    string argv1 = argv[1];
    if (argv1=="silent") verbose = false;
  }
  vector<struct STestData> cases;
  FillTestCases(cases);

  for (unsigned i=0; i<cases.size(); i++){
    if (!RunAndCheck(cases[i])){
      if (verbose) cout << "triangulation failed\n";
      return -1;
    }
    //if (verbose) cout << "." << flush;
  }

  return 0;
}


bool RunAndCheck(const struct STestData &data)
{
  Triangulation tri;
  HomgPoint3D p3d, tmp;
  
  int res = tri.TriangulateProjective(Get(data.proj1), Get(data.proj2), 
                                      data.p1, data.p2, p3d);

  if (verbose) cout << "\nexpected: "<<data.p3d
                    << "\ngot:      "<<p3d<<endl;

  if (res != data.triangulation_result){
    if (verbose) cout << "result mismatch: expected: "
                      <<data.triangulation_result<<"  got: "<<res<<"\n";
    return false;
  }
  tmp = data.p3d;
  BIASASSERT(!Equal(tmp.NormL2(), 0.0));
  tmp /= tmp.NormL2();

  BIASASSERT(!Equal(p3d.NormL2(), 0.0));
  p3d /= p3d.NormL2();
  // difference vector
  Vector4<double> diff = tmp - p3d;

  double eps = numeric_limits<double>::epsilon() * 5.0;
  if (diff.NormL2() > eps){
    if (verbose) cout << "result point is incorrect: got "<<p3d
                      <<"\n  expected: "<<tmp<<"\n";
    return false;
  }


  return true;
}


void FillTestCases(vector<struct STestData> &cases)
{
  cases.clear();
  
  Vector3<double> C1(0., 0., 0.), C2(1., 0., 0.);
  KMatrix K(MatrixIdentity);
  Quaternion<double> Q1(0., 0., 0., 1.), Q2(0., 0., 0., 1.);
  SharedPtr<ProjectionParametersPerspective> proj1, proj2;
  proj1 = SharedPtr<ProjectionParametersPerspective>(new ProjectionParametersPerspective);
  proj2 = SharedPtr<ProjectionParametersPerspective>(new ProjectionParametersPerspective);
  proj1->SetK(K);
  proj1->SetC(C1);
  proj1->SetQ(Q1);
  proj2->SetK(K);
  proj2->SetC(C2);
  proj2->SetQ(Q2);

  const int width = 640, height = 480;
  const double phiOffs=0.0, thetaOffs=0., dPhi=0.001, dTheta=0.001;
  SharedPtr<ProjectionParametersSphericalSimple> projsph1, projsph2;
  projsph1 = SharedPtr<ProjectionParametersSphericalSimple>(new ProjectionParametersSphericalSimple(phiOffs, thetaOffs, dPhi, dTheta, width, height));
  projsph2 = SharedPtr<ProjectionParametersSphericalSimple>(new ProjectionParametersSphericalSimple(phiOffs, thetaOffs, dPhi, dTheta, width, height));
  projsph1->SetC(C1);
  projsph1->SetQ(Q1);
  projsph2->SetC(C2);
  projsph2->SetQ(Q2);

  struct STestData data;

  data.proj1 = proj1;
  data.proj2 = proj2;

  // exact intersection
  data.p1.Set(0., 0., 1.);
  data.p2.Set(-0.1, 0., 1.);
  data.p3d.Set(0., 0., 10., 1.);
  data.triangulation_result = 0;
  cases.push_back(data);

  // exact intersection
  data.p1.Set(0.1, 0., 1.);
  data.p2.Set(0.0, 0., 1.);
  data.p3d.Set(1., 0., 10., 1.);
  data.triangulation_result = 0;
  cases.push_back(data);
  
  // distant point
  data.p1.Set(0.001, 0., 1.);
  data.p2.Set(0.0, 0., 1.);
  data.p3d.Set(1., 0., 1000., 1.);
  data.triangulation_result = 0;
  cases.push_back(data);
  
  // resulting point behind camera
  data.p1.Set(0.0, 0.0, 1.);
  data.p2.Set(0.1, 0.0, 1.);
  data.p3d.Set(0., 0., -10., 1.);
  data.triangulation_result = TRIANG_RES_BEHIND_CAMERA1;
  cases.push_back(data);
  
  // resulting point at infinity
  data.p1.Set(0.0, 0.0, 1.);
  data.p2.Set(0.0, 0.0, 1.);
  data.p3d.Set(0., 0., 1., 0.);
  data.triangulation_result = TRIANG_RES_PARALLEL;
  cases.push_back(data);
  
   // lines are not parallel, but closest point is between camera centers
  data.p1.Set(0.0, 0.1, 1.);
  data.p2.Set(0.0, 0.0, 1.);
  data.p3d.Set(0.5, 0., 0., 1.);
  data.triangulation_result = 0;
  cases.push_back(data);
  
  // lines do not intersect exactly
  data.p1.Set(0.1, 0.01, 1.);
  data.p2.Set(-0.1, -0.01, 1.);
  data.p3d.Set(0.5, 0., 4.950495049504950, 1.);
  data.triangulation_result = 0;
  cases.push_back(data);

  // now spherical projection parameters
  data.proj1 = projsph1;
  data.proj2 = projsph2;

  // optical axes
  data.p1.Set(M_PI/2./dTheta, 0.0, 1.);
  data.p2.Set(M_PI/2./dTheta, 0.0, 1.);
  data.p3d.Set(0., 0., 1., 0.);
  data.triangulation_result = TRIANG_RES_PARALLEL;
  cases.push_back(data);
  
  // exact intersection
  data.p1.Set(M_PI/2./dTheta, 0.0, 1.);
  data.p2.Set(3.*M_PI/4./dTheta, 0.0, 1.);
  data.p3d.Set(0., 0., 1., 1.);
  data.triangulation_result = 0;
  cases.push_back(data);
  
  // behind the camera
  data.p1.Set(M_PI/2./dTheta, 0.0, 1.);
  data.p2.Set(M_PI/4./dTheta, 0.0, 1.);
  data.p3d.Set(0., 0., -1., 1.);
  data.triangulation_result = TRIANG_RES_BEHIND_CAMERA1;
  cases.push_back(data);
  
  
}