#include "Geometry/FMatrix.hh"
#include "Geometry/RMatrix.hh"
#include "Geometry/EMatrix.hh"
#include "Base/Geometry/KMatrix.hh"
#include "../FMatrixEstimation.hh"
#include "../PMatrixEstimation.hh"
#include "../PMatrixLinear.hh"
#include "../Triangulation.hh"
#include "../../Base/Math/Random.hh"
#include "Utils/ThreeDOut.hh"
#include "Utils/Param.hh"
using namespace BIAS;
using namespace std;
int main(int argc, char *argv[]) {
p->
AddParamDouble(
"princPointX",
"x-coordinate of principle point", 199.5);
p->
AddParamDouble(
"princPointY",
"y_coordinate of principle point", 199.5);
p->
AddParamDouble(
"rotationX",
"Rotation of second cam in x-direction", 12.0);
p->
AddParamDouble(
"rotationY",
"Rotation of second cam in y-direction", 20.0);
p->
AddParamDouble(
"rotationZ",
"Rotation of second cam in z-direction", -2.0);
p->
AddParamDouble(
"transX",
"Translation of second cam in x-direction", -1.0/sqrt(2.0));
p->
AddParamDouble(
"transY",
"Translation of second cam in y-direction", 1.0 /sqrt(2.0));
p->
AddParamDouble(
"transZ",
"Translation of second cam in z-direction", 0.0);
p->
AddParamDouble(
"noiseUpper",
"Upper bound for noise on points in pixels", 0.0);
p->
AddParamDouble(
"noiseLower",
"Lower bound for noise on points in pixels", -0.0);
if (argc == 2) {
if (res != 0) {
cout << "Could not read parameter file, using default parameters instead " <<res << endl;
}
}
if (argc == 2) {
if (res != 0) {
BIASERR("Error could not write parameter file to: " << argv[1] << res);
}
} else {
if (res != 0) {
BIASERR("Error could not write parameter file to: " << "autoCalib.param" << res);
}
}
std::vector<FMatrix> vecF;
C1[0] = 0.0;
C1[1] = 0.0;
C1[2] = 0.0;
K[2][2] = 1;
cout << "R1 " << R1 << endl;
cout << "R2 " << R2 << endl;
cout << "C1 " << C1 << endl;
cout << "C2 " << C2 << endl;
cout << "K " << K << endl;
cout << "P1 " << P1 << endl;
cout << "P2 " << P2 << endl;
vector<HomgPoint3D> points, pointsComp;
vector<HomgPoint2D> p1, p2;
int numPoints = 50;
for (int p=0; p<numPoints; p++) {
cout << "NO. " << points[p] << endl;
p1.push_back(P1 * points[p]);
p1[p].Homogenize();
p2.push_back(P2 * points[p]);
p2[p].Homogenize();
}
for (int i = 0; i < numPoints; i++) {
}
bool NormalizeHartley = true;
vecF.resize(1);
vecF.clear();
cout <<
"True F matrix ----------- residual error " << F_gt.
GetResidualError(p1, p2) << endl;
cout <<
"Estimated F matrix ------ residual error " << F.
GetResidualError(p1, p2) << endl;
int res = pEstimator.
AutoCalib(F, width, height, p1, p2, P1Comp, P2Comp);
if (res != 0) {
BIASERR("Couldn't determine f, P1 and P2" << res);
return -1;
}
cout << "Results of AutoCalib: (true angle of view " << atan(width/(2.0*focalLength)) *180.0/M_PI
<< ")" << endl;
cout << "P1 " << P1Comp << endl;
cout << "P2 " << P2Comp << endl;
cout <<
"Resuting Kmatrix " << P2Comp.
GetK() << endl;
cout <<
"Resulting C vectors " << P1Comp.
GetC() <<
" "<< P2Comp.
GetC() <<endl;
cout <<
"Resulting Rmatrix " << P2Comp.
GetR() << endl;
double phiX, phiY, phiZ;
cout << "Rotation angles " << phiX*180.0/M_PI << " " << phiY*180.0/M_PI << " " << phiZ*180.0/M_PI
<< endl;
cout << "ERRORS " << endl;
cout <<
"Focal length " << (P2Comp.
GetK()).GetFx() << endl;
cout << "Diff P2 " << P2Comp - P2 << endl;
for (int i = 0; i < numPoints; i++) {
triangulator.
Triangulate(P1Comp, P2Comp, p1[i], p2[i], tempPoint);
}
for (int i = 0; i < numPoints; i++) {
}
for (int i = 0; i < numPoints; i++) {
}
threeDOut.
AddPMatrix(P1, 400, 400, RGBAuc_WHITE_OPAQUE, 0.1);
return 0;
}