#include "bias_config.h"
#include "Geometry/EpipoleEstimation.hh"
#include "Base/Math/Random.hh"
#include "Base/Geometry/KMatrix.hh"
#include "Base/Geometry/RMatrixBase.hh"
#include "Geometry/FMatrix.hh"
#include "Geometry/PMatrix.hh"
#include "Base/Debug/TimeMeasure.hh"
#include <iostream>
#include <iomanip>
#define EPIPOLE_FACTOR 15.0
#define BACK 0
#define LINE 70
#define MATCH 255
using namespace BIAS;
using namespace std;
vector<BIAS::HomgPoint2D> match1, match2;
void CreateMatches2(vector<HomgPoint2D>& match1, vector<HomgPoint2D>& match2,
unsigned int match_count, unsigned int width,
unsigned int height, double spatial_noise_sigma,
double r_ang)
{
K[0][0]=K[1][1]=840.0;
K[0][2]=(width-1.0)/2.0;
K[1][2]=(height-1.0)/2.0;
vector<PMatrix> pvec;
pvec.push_back(P1);
pvec.push_back(P2);
true_epipole = P2 * C1;
vector<vector<HomgPoint2D> > i_matches, n_matches;
vector<HomgPoint3D> p3d;
CreateMatches(pvec, match_count, width, height, spatial_noise_sigma,
i_matches, n_matches, p3d, 2.0, -30.0, 30.0, -30.0, 30.0,
-30.0, 30.0, false);
match1.resize(match_count);
match2.resize(match_count);
for (unsigned i=0; i<match_count; i++){
match1[i]=n_matches[i][0];
match2[i]=n_matches[i][1];
}
}
void CreateMatches(vector<BIAS::HomgPoint2D>& match1,
vector<BIAS::HomgPoint2D>& match2,
unsigned int width, unsigned int height,
double maxdisparity, double mindisparity,
double spatial_noise_sigma)
{
double dist, p1epipoledist;
match1.clear();
match2.clear();
p1[2]=p2[2]=1.0;
for (unsigned int i=0; i<match_count; i++){
do {
p1epipoledist=sqrt((epipole[0]-p1[0])*(epipole[0]-p1[0])+
(epipole[1]-p1[1])*(epipole[1]-p1[1]));
while (dist>=p1epipoledist);
p2[0]=p1[0]+(epipole[0]-p1[0])*dist/p1epipoledist
p2[1]=p1[1]+(epipole[1]-p1[1])*dist/p1epipoledist
p2=K*R*Kinv*p2;
} while (p2[0]<0.0 || p2[0]>(double)(width-1) ||
p2[1]<0.0 || p2[1]>(double)(height-1));
match1.push_back(p1);
match2.push_back(p2);
}
}
void AddErr(double& mean_ep_dist, double& mean_ang, double& mean_el_dist)
{
mean_ep_dist += estimated_epipole.
Distance(true_epipole);
vector<HomgPoint2D>::iterator it1, it2;
double ma=0.0, md=0.0;
int c=0;
for (it1=match1.begin(), it2=match2.begin();
it1!=match1.end() && it2!=match2.end(); it1++, it2++, c++){
l1.
Set(true_epipole, *it2);
l2.
Set(estimated_epipole, *it2);
ma+=fabs(atan2(l1[1], l1[0])-atan2(l2[1], l2[0]));
md+=(*it1).ScalarProduct(E*(*it2));
}
mean_ang=ma/(double)c;
mean_el_dist=md/(double)c;
}
int main(int argc, char *argv[])
{
ofstream os("errcp3.txt");
ofstream os2("errls3.txt");
double spatial_noise_sigma=0.0;
double max_disparity=50.0;
double mindisparity=2.0;
unsigned int match_count=80;
unsigned int width=640, height=480;
int count=100;
double mean_ep_dist_ls, mean_ep_dist_cp, mean_ang_ls, mean_ang_cp;
double mean_el_dist_ls, mean_el_dist_cp;
EpipoleEstimation ee;
center.
Set(width/2, height/2);
if (argc>=2) match_count=atoi(argv[1]);
if (argc>=3) spatial_noise_sigma=atof(argv[2]);
K.Fill(0.0);
K[0][0]=K[1][1]=840.0;
K[0][2]=double(width)/2.0;
K[1][2]=double(height)/2.0;
K[2][2]=1.0;
true_epipole[2]=1.0;
true_epipole[0]=
(EPIPOLE_FACTOR+1.0)*(double)width);
EPIPOLE_FACTOR+1.0*(double)height);
CreateMatches2(match1, match2, match_count, width, height,
spatial_noise_sigma, 0.0);
os <<"# EpipoleEstimationCrossProduct\n"
<<"# true epipole: "<<true_epipole[0]<<" "<<true_epipole[1]
<<" "<<true_epipole[2]<<endl
<<"# "<<match_count<<" matches"
<<"\n# max disparity: "<<max_disparity
<<"\n# min disparity: "<<mindisparity<<endl;
os2 <<"# EpipoleEstimationCrossProduct\n"
<<"# true epipole: "<<true_epipole[0]<<" "<<true_epipole[1]
<<" "<<true_epipole[2]<<endl
<<"# "<<match_count<<" matches"
<<"\n# max disparity: "<<max_disparity
<<"\n# min disparity: "<<mindisparity<<endl;
double err_r;
for (spatial_noise_sigma=0.0; spatial_noise_sigma<1.05;
spatial_noise_sigma+=0.1){
os <<endl<<endl<<"# spatial noise: "<<spatial_noise_sigma
<<"\n# spat-nois\trot-err(DEG)\trot-err(RAD)\tabs-dist-err"
<<"\tabs-angle-err\tabs-el-dist-err\n";
os2 <<endl<<endl<<"# spatial noise: "<<spatial_noise_sigma
<<"\n# spat-nois\trot-err(DEG)\trot-err(RAD)\tabs-dist-err"
<<"\tabs-angle-err\tabs-el-dist-err\n";
for (err_r=1e-3; err_r<=0.5; err_r*=2.0){
mean_ep_dist_ls = mean_ep_dist_cp = mean_ang_ls = mean_ang_cp =
mean_el_dist_ls = mean_el_dist_cp = 0.0;
for (int c=0; c<count; c++){
CreateMatches2(match1, match2, match_count, width, height,
spatial_noise_sigma, err_r);
if (ee.CrossProductLeastSquaresSVD(match1,match2,
estimated_epipole)!=0){
BIASERR("error estimating epipole");
return 0;
}
AddErr(mean_ep_dist_cp, mean_ang_cp, mean_el_dist_cp);
if (ee.MatchLengthWeightedLeastSquares(match1, match2,
estimated_epipole)!=0){
BIASERR("error estimating epipole");
return 0;
}
AddErr(mean_ep_dist_ls, mean_ang_ls, mean_el_dist_ls);
cout <<".";
cout.flush();
}
mean_ep_dist_ls /= count;
mean_ep_dist_cp /= count;
mean_ang_ls /= count;
mean_ang_cp /= count;
mean_el_dist_ls /= count;
mean_el_dist_cp /= count;
os2 <<setw(6)<<spatial_noise_sigma<< "\t"<< setw(6) << err_r/2/M_PI*360
<< setw(12) << err_r << "\t" << setw(12)
<<mean_ep_dist_ls << "\t" << setw(12)
<<mean_ang_ls<<"\t"<<mean_el_dist_ls<<endl;
os <<setw(6)<<spatial_noise_sigma<< "\t"<< setw(6) << err_r/2/M_PI*360
<< setw(12) <<err_r << "\t" << setw(12)
<<mean_ep_dist_cp << "\t" << setw(12)
<<mean_ang_cp<<"\t"<<mean_el_dist_cp<<endl;
cout <<endl;
cout <<setw(12)<<spatial_noise_sigma<<setw(12)<<err_r<<"\t"<<setw(12)
<<mean_ep_dist_ls
<< "\t"<<setw(12)<<mean_ang_ls<<"\t"<<mean_el_dist_ls<<endl;
cout <<setw(12)<<spatial_noise_sigma<<setw(12)<<err_r<<"\t"<<setw(12)
<<mean_ep_dist_cp
<< "\t"<<setw(12)<<mean_ang_cp<<"\t"<<mean_el_dist_cp<<endl;
}
}
os2.close();
os.close();
return 0;
}