#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()
{
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(407.0, 100.0, 1.0), point2d_im2(419.0, 90.0, 1.0);
HomgPoint3D p3, p3d_real(5.087403297, -8.891034126, 26.40314484);
double dist, angle;
cerr <<"projecting "<<p3d_real<<" with pleft and pright: "<<endl;
ptl = pleft * p3d_real;
cerr << "left: "<< ptl << endl;
ptr = pright * p3d_real;
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){
cerr << "error triangulating using GetNormRayWorldCoo"<<endl;
} else {
cerr << "triangulated: "<<p3<<endl;
}
cerr <<endl<<"triangulating from theoretical points \n"<<ptl<<"\nP: "
<<pleft<<endl <<point2d_im2<<"\nP: "<<ptr<<endl;
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;
}