ExampleTriangulate2.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 ExampleTriangulate2.cpp
   @relates Triangulation
   @brief example demonstrating triangulation
   @ingroup g_examples
   @author ischiller 10/2009 
*/


#include <Geometry/Triangulation.hh>
#include <Geometry/RMatrix.hh>
#include <Geometry/PMatrix.hh>
#include <Base/Geometry/KMatrix.hh>

#include <fstream>
#include <iostream>

using namespace BIAS;
using namespace std;

//int main(int argc, char *argv[]) 
int main() 
{
  PMatrix 
//    pleft("415.692215            2.8e-05                320                 -0 
//                  0         415.692207         239.999964                 -0 
//                  0                  0                  1                 -0"), 
//    pright("415.692215            2.8e-05                320        -443.520012 
//                  0         415.692207         239.999964        -332.640009 
//                  0                  0                  1             -1.386");
    pleft("415.692177      2.7975296e-05                320                 -0\
                 0         415.692207        239.9999637                 -0\
                 0        8.74228e-08                  1                 -0 "),
    pright("415.692177      2.7975296e-05                320            -633.6\
                 0         415.692207        239.9999637        -475.199928\
                 0        8.74228e-08                  1              -1.98");
  //HomgPoint2D point2d_im1(378.0, 86.0, 1.0), point2d_im2(379.0, 83.0, 1.0);
  HomgPoint2D point2d_im1(407.0, 100.0, 1.0), point2d_im2(419.0, 90.0, 1.0);
  HomgPoint2D ptl, ptr;
  //HomgPoint3D p3, p3d_real(11.40732193, -30.37988853, 81.99732971);
  HomgPoint3D p3, p3d_real(5.087403297, -8.891034126, 26.40314484);
  Vector3<double> eucl3d;
  Matrix3x3<double> cov;
  double dist, angle;
  Triangulation triangulator;
  cerr <<"projecting "<<p3d_real<<" with pleft and pright: "<<endl;
  ptl = pleft * p3d_real;
  ptl.Homogenize();
  cerr << "left: "<< ptl << endl;

  ptr = pright * p3d_real;
  ptr.Homogenize();
  cerr << "right: "<< ptr << endl;

  cerr <<endl<< "triangulating from \n"<<point2d_im1<<"\nP: "<<pleft<<endl
       <<point2d_im2<<"\nP: "<<pright<<endl;
  if (triangulator.Triangulate2D(pleft, pright,  point2d_im1,  point2d_im2, 
                               p3, cov, dist, angle)!=0){ 
    cerr << "error triangulating using GetNormRayWorldCoo"<<endl;
  } else {
    cerr << "triangulated: "<<p3<<"\n with covariance matrix: "<<cov
         <<"\ndistance: "<<dist<<"\tangle: "<<angle<<endl;
  }
  
  cerr <<endl<< "triangulating with svd from points \n"<<point2d_im1<<"\nP: "
       <<pleft<<endl<<point2d_im2<<"\nP: "<<pright<<endl;
  if (triangulator.Triangulate(pleft, pright, point2d_im1,  point2d_im2,
                               eucl3d)!=0){
    p3.Set(eucl3d);
    cerr << "error triangulating using GetNormRayWorldCoo"<<endl;
  } else {
    p3.Set(eucl3d);
    cerr << "triangulated: "<<p3<<endl;
  }
  

  cerr <<endl<<"triangulating from theoretical points \n"<<ptl<<"\nP: "
       <<pleft<<endl <<point2d_im2<<"\nP: "<<ptr<<endl;
  if (triangulator.Triangulate2D(pleft, pright,  ptl,  ptr, 
                                 p3, cov, dist, angle)!=0){ 
    cerr << "error triangulating using GetNormRayWorldCoo"<<endl;
  } else {
    cerr << "triangulated: "<<p3<<"\n with covariance matrix: "<<cov
         <<"\ndistance: "<<dist<<"\tangle: "<<angle<<endl;
  }
  

  return 0;
}