#include <Base/Common/BIASpragma.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/RMatrix.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Base/Math/Random.hh>
#include <vector>
#include <Geometry/PMatrixLinear.hh>
using namespace BIAS;
using namespace std;
int main()
{
cout << "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ ExamplePMatrix $$$$$$$$$$$$$$$$$$$$$$"
<< endl;
K *= 100.0;
K[0][2] = 384;
K[1][2] = 256;
K[2][2] = 1.0;
double angle = 1.0;
BIASERR("Error computing Hinf");
};
cout<<
"C is "<<P_current.
GetC()<<
" R is "<<P_current.
GetR()
<<
" K is "<<P_current.
GetK()<<
" Hinf is "<<Hinf<<endl;
cout<<
"C is "<<Pose.
GetC()<<
" R is "<<Pose.
GetR()
<<
" K is "<<Pose.
GetK()<<endl;
bool TestP = false;
if (TestP) {
for (unsigned int i=0; i<4; i++)
for (unsigned int j=0; j<3; j++) {
double noise = 0.995+(0.01*rand()/(double)RAND_MAX);
P_current[j][i] *= noise;
cout << noise <<" ";
}
P_current *= 7.5;
cout <<"Pcurrent is "<<P_current<<endl;
cout <<"Pcurrent after normalizing is "<<P_current<<endl;
BIASERR("Error computing Hinf");
};
cout<<
"C is "<<P_current.
GetC()<<
" R is "<<P_current.
GetR()
<<
" K is "<<P_current.
GetK()<<
" Hinf is "<<Hinf<<endl;
}
cout <<" +++++++++++++++++++++++++++++++++++++++++++++++++++++++++"<<endl;
cout <<" ++++++++++++++ linear p estimation ++++++++++++++++++++++"<<endl;
vector<HomgPoint3D> vecX;
vector<HomgPoint2D> vecx;
vector<HomgPoint3D*> vecpX;
vector<HomgPoint2D*> vecpx;
unsigned int n=6;
vecX.reserve(n);
vecx.reserve(n);
bool WantPlane=true, WantNoise=true;
unsigned int NumberOffPlane = 0;
for (unsigned int i=0; i<n; i++) {
if (WantPlane) {
X = a*weight1 + b*weight2 + c*(1.0-weight1-weight2);
X[3] = 1.0;
} else {
}
if (i<NumberOffPlane) {
}
cout <<"using point X="<<X<<endl;
x = P_current * X;
if (WantNoise) {
}
vecX.push_back(X);
vecx.push_back(x);
}
for (unsigned int i=0; i<n; i++) {
vecpX.push_back(&vecX[i]);
vecpx.push_back(&vecx[i]);
}
cout <<"Pose estimation:"<<endl;
cout <<"Pose is "<<Pose<<endl;
cout <<"P_current is "<<P_current<<endl;
cout <<"Diff is "<<Pose-P_current<<endl;
cout<<endl;
cout <<"complete PMatrix estimation:"<<endl;
if (!PLin.
Compute(vecpX, vecpx, Pose)) BIASERR(
"failed !");
if (Pose[2][0]*P_current[2][0]<0) Pose *= -1.0;
cout <<"Pose is "<<Pose<<endl;
cout <<"P_current is "<<P_current<<endl;
cout <<"Diff is "<<Pose-P_current<<endl;
cout<<endl;
return 0;
}