#include <Geometry/Triangulation.hh>
#include <Geometry/RMatrix.hh>
#include <Geometry/PMatrix.hh>
#include <Base/Geometry/KMatrix.hh>
#include "Geometry/CovMatrix3x3.hh"
#include <fstream>
#include <iostream>
using namespace BIAS;
using namespace std;
int main()
{
PMatrix pleft,pleftScale,pleftScaleTriang,pright,prightScale,prightScaleTriang;
ifstream infileL("left.matrix");
ifstream infileR("right.matrix");
K[0][0]=K[1][1]=480;
K[0][2]=640/2.0;
K[1][2]=480/2.0;
Cleft.
Set(0.0, 0.0, 0.0);
Cright.
Set(2.5, 0.0, -5.0);
double scale = 10.0;
CleftScaleShift=scale*Cleft;
CleftScaleShift +=shift;
CrightScaleShift=scale*Cright;
CrightScaleShift+=shift;
pleftScale.
Compose(K, Rscale, CleftScaleShift);
prightScale.
Compose(K, Rscale, CrightScaleShift);
if (infileL) infileL>>pleft;
if (infileR) infileR>>pright;
cout<<"Read:"<<endl<<pleft<< endl <<pleftScale<<endl<<pright<<endl;
srand(0);
for (int i = 0; i <=5; i++){
for (int k=0; k<3; k++)
point3d[k]=10.0*rand()/(RAND_MAX+1.0)-5;
point3d[2]+=6;
point3d[3]=1;
point2d_im1s(pleftScale*point3d);
point2d_im2s(prightScale*point3d);
point2d_im1.Homogenize();point2d_im1s.
Homogenize();
if (triangulator.
Triangulate(pleft, pright, point2d_im1, point2d_im2,
eucl3d)!=0){
cerr << "error triangulating using SVD"<<endl;
}
cout << "p3 =" << p3;
triangulator.
Triangulate(pleftScaleTriang, prightScaleTriang,
point2d_im1s, point2d_im2s, eucl3d);
vp3s = Rscale*vp3s*scale+shift;
cout << "p3 with scaled, rot baseline= " << p3s << ", vp3s = "
<< vp3s << endl;
cout << "error is [" << (vp3s[0]-p3[0])/p3[0] << ", " << (vp3s[1]-p3[1])/p3[1]
<< ", " <<(vp3s[2]-p3[2])/p3[2] << "] " << endl;
}
cout<<"Now triangulation of:"<<pl<<" "<<pr<<endl;
cout<<"result of Triangluation: "<<
triangulator.
Triangulate(pleft, pright, pl, pr, eucl3d)<<endl;
cout<<"p3: "<<p3<<endl;
a1[0]= 0; a2[0]= 0;
a1[1]= 0; a2[1]= 0;
a1[2]= 0; a2[2]= 1;
b1[0]= 0; b2[0]= 1;
b1[1]= 1; b2[1]= 1;
b1[2]= 0; b2[2]= 0;
cout<<"Interscection of: a and b:"<<
cout<<"Interscection of: b and a:"<<
return 0;
}