#include <Geometry/Triangulation.hh>
#include <Base/Math/Random.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Geometry/PMatrix.hh>
using namespace BIAS;
using namespace std;
int main(int argc, char *argv[])
{
double sigma=2.5;
int num=100, nump=10;
bool read=false, finished=false;
double osum=0.0, ksum=0.0, ssum=0.0, osum2=0.0, ksum2=0.0, ssum2=0.0;
int onum=0, knum=0, snum=0;
if (argc==2) {
read=true;
}
if (read){
num=1;
}
K[0][0]=K[1][1]=840.0;
K[0][2]=320.0;
K[1][2]=240.0;
for (int p=0; p<nump; p++){
if (read){
ifstream is(argv[1]);
is >> K >> R0 >> R1 >> C0 >> C1 >> np0 >> np1 >> p3d >> gtp0 >> gtp1;
is.close();
cout << K << R0 << R1 << C0 << C1 << np0 << np1 << p3d<<gtp0<<gtp1<<endl;
}
for (int i=0; i<num; i++){
gtp0=P0*p3d;
gtp1=P1*p3d;
if (num<5){
cout << "\ntrue 3d point : "<<p3d<<endl;
cout << "true correspondences : "<<gtp0<<" <--> "<<gtp1<<endl;
}
if (read){
p0=np0;
p1=np1;
} else {
p0=gtp0;
p1=gtp1;
np0=p0;
np1=p1;
}
if (num<5){
cout << "noisy correspondences (sigma = "<<setw(5)<<sigma<<") : "<<p0<<" <--> "
<<p1<<endl;
}
if (tri.
Optimal(P0, P1, p0, p1, tp3d)==0){
if (num<5){
cout << "true 3d point : "<<p3d<<endl;
cout << "true correspondences : "<<gtp0<<" <--> "<<gtp1<<endl;
cout << "noisy correspondences (sigma = "<<setw(5)<<sigma<<") : "<<np0<<" <--> "
<<np1<<endl;
}
cout << "corrected correspondences : "<<p0<<" <--> "<<p1<<endl;
cout << "projected triangulated point : "<<p0<<" <--> "<<p1<<endl;
cout << "optimal triangulated 3D point : "<<tp3d<<endl;
}
cout << "optimal euclidean dist. in 3D : "<<(tp3d-p3d).NormL2()
<<"\teucl. dist in images: "<<(np0-p0).NormL1()<<" "
<<(np1-p1).NormL1()<<endl;
osum+=(tp3d-p3d).NormL2();
osum2+=(tp3d-p3d).NormL2()*(tp3d-p3d).NormL2();
onum++;
if ((tp3d-p3d).NormL2()>1.0){
cout << K << R0 << R1 << C0 << C1 << np0 << np1<<p3d<<gtp0<<gtp1<<endl;
ofstream of("deb.txt");
of << K << R0 << R1 << C0 << C1 << np0 << np1 << p3d<<gtp0<<gtp1<<endl;
of.close();
}
} else {
cout << "optimal triangulation failed\n";
}
p0=np0;
p1=np1;
if (num<5){
cout << "kanatani triangulated 3D point : "<<tp3d<<endl;
}
cout << "kanatani euclidean dist. in 3D: "<<(tp3d-p3d).NormL2()<<endl;
ksum+=(tp3d-p3d).NormL2();
ksum2+=(tp3d-p3d).NormL2()*(tp3d-p3d).NormL2();
knum++;
} else {
cout << "kanatani triangulation failed\n";
}
p0=np0;
p1=np1;
double dist, ang;
if (num<5){
cout << "2D triangulated 3D point : "<<tp3d<<endl;
}
cout << "2D euclidean dist. in 3D : "<<(tp3d-p3d).NormL2()<<"\tdist: "
<<dist<<"\tang: "<<ang<<endl;
ssum+=(tp3d-p3d).NormL2();
ssum2+=(tp3d-p3d).NormL2()*(tp3d-p3d).NormL2();
snum++;
} else {
cout << "2D triangulation failed\n";
}
if (finished) return -1;
}
}
if (onum>1 && knum>1 && snum>1){
cout << "optimal calculated "<<onum<<" times with mean error "
<<osum/(double)onum<<" +- "
<< (osum2-(osum*osum)/(double)onum)/(double)(onum-1.0)
cout << "kanatani calculated "<<knum<<" times with mean error "
<<ksum/(double)knum<<" +- "
<< (ksum2-(ksum*ksum)/(double)knum)/(double)(knum-1.0)
cout << "2D calculated "<<snum<<" times with mean error "
<<ssum/(double)snum<<" +- "
<< (ssum2-(ssum*ssum)/(double)snum)/(double)(snum-1.0)
}
return 0;
}