#include "Geometry/CamPoseCalib.hh"
#include "Base/Math/Random.hh"
using namespace BIAS;
using namespace std;
int nrPoints=100;
int percentOutlier=0;
double boxSize=10, boxDepth=1, boxDistance=30;
double sigmaNoise=0.5, outlier=5;
int main(int, char **)
{
std::vector<Vector3<double> > mps;
for (int i=0;i<nrPoints;i++) {
boxDistance+boxDepth/2.0);
mps.push_back(mp);
}
double fx=912/2.0,fy=912/2.0,cx=383/2.0,cy=287/2.0;
cx=343/2.0,cy=247/2.0;
D_BIAS_CAMPOSE_ESTIMATE_ITER);
cout<<endl<<endl<<
"---- estimating external parameters --------"<<endl;
K[0][0]= fx; K[0][2]= cx;
K[1][0]= 0; K[1][1]= fy; K[1][2]= cy;
K[0][0]= fx ; K[0][2]= cx;
K[1][1]= fy; K[1][2]= cy;
R.
SetXYZ(0, -45.0/180.0*M_PI, 0/180.0*M_PI);
std::vector<Vector2<double> > tps;
for (unsigned int i=0;i<mps.size();i++) {
tps.push_back(y);
}
if (sigmaNoise>1E-15)
for (unsigned int i=0;i<tps.size();i++) {
}
if (percentOutlier>0.000001)
for (unsigned int i=0;i<tps.size()/(percentOutlier*100);) {
if (fabs(offset)>(3*sigmaNoise)) {
tps[i][0]+=offset;
i++;
}
if (fabs(offset)>(3*sigmaNoise)) {
tps[i][1]+=offset;
i++;
}
}
for (unsigned int i=0;i<mps.size();i++) {
}
cout<<"estimated R:"<<resR<<endl;
double angle;
cout<<"Rotation axis:"<<axis<<" angle:"<<angle/M_PI*180.0<<endl;
" y5:"<<tps[4]<<" estimatedP*mp5:"<<yEst<<endl;
cout<<"Real K:"<<K<<endl
<<
"Estimated K:"<<Pest.
GetK()<<endl;
cout<<"CoVariance of Estimation:"<<coVar<<endl;
cout<<endl<<endl<<
"---- Reestimating with principal point and Huber weighting function ----"
<<endl;
cout<<"estimated R:"<<resR<<endl;
cout<<"Rotation axis:"<<axis<<" angle:"<<angle/M_PI*180.0<<endl;
" y5:"<<tps[4]<<" estimatedP*mp5:"<<yEst<<endl;
cout<<"Real K:"<<K<<endl
<<
"Estimated K:"<<Pest.
GetK()<<endl;
cout<<"CoVariance of Estimation:"<<coVar<<endl;
}