#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"
using namespace BIAS;
using namespace std;
int main() {
R2.
SetXYZ(0, 10.0*M_PI/180.0, 0);
C1[0] = 0.0;
C1[1] = 0.0;
C1[2] = 0.0;
C2[0] = -1.0;
C2[1] = 0.0;
C2[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;
cout << "Randomized correspondences are "<<endl;
int numPoints = 20;
double noise = 0.5;
for (int p=0; p<numPoints; p++) {
p1.push_back(P1 * points[p]);
if(noise > 0){
}
p1[p].Homogenize();
cout << "{ "<<p1[p]<<" ";
p2.push_back(P2 * points[p]);
if(noise > 0){
}
p2[p].Homogenize();
cout << p2[p]<<" } "<<endl;
}
int numPointsOnLines = 20;
vector<HomgPoint3D> line1, line2, line1Comp, line2Comp;
vector<HomgPoint2D> l1P1, l1P2, l2P1, l2P2;
double randLambda = 0.0;
for (int i = 0; i < numPointsOnLines; i++) {
line1.push_back(
HomgPoint3D(-1.0 + randLambda, 1.0 + randLambda, 1.0
+ randLambda));
l1P1.push_back(P1 * line1[i]);
l1P1[i].Homogenize();
l1P2.push_back(P2 * line1[i]);
l1P2[i].Homogenize();
line2.push_back(
HomgPoint3D(1.0 + randLambda, -1.0 + randLambda, 1.0
+ randLambda));
l2P1.push_back(P1 * line2[i]);
l2P1[i].Homogenize();
l2P2.push_back(P2 * line2[i]);
l2P2[i].Homogenize();
}
bool NormalizeHartley = true;
cout << "F " << F << endl;
cout << "F " << F_opt << endl;
cout << "testCompF " << testCompF << endl;
KFalsch[1][1] = KFalsch[0][0];
cout << "disturbed kmatrix " << KFalsch << endl;
cout <<
"Epipole1 " << Epipole1.
Homogenize() <<
" Epipole2 "
double BaselineMagnitude = (C1-C2).NormL2();
if (P2Comp.
GetC().ScalarProduct(C2) < 0.0)
P1Comp = KFalsch * P1Comp;
P2Comp = KFalsch * P2Comp;
cout << "P2COMP IS " << P2Comp << endl;
cout<< "Resulting C vectors " << P1Comp.GetC() <<" " << P2Comp.GetC()
<<endl;
double almostZero = 1e-10;
cout << "resulting rotation matrices " << P1Comp.GetR() << " " << REst
<< endl;
cout << "Det of P2 " << det << endl;
cout <<
id <<
" identity test: " <<
id.
IsIdentity(1) << endl;
double phiX, phiY, phiZ;
cout << "Rotation angles " << phiX*180.0/M_PI << " " << phiY*180.0 /M_PI
<< " " << phiZ*180.0/M_PI << endl;
bool isOrthonormal = true;
if (fabs(scalarProduct1) >= almostZero)
isOrthonormal = false;
if (fabs(scalarProduct2) >= almostZero)
isOrthonormal = false;
if (fabs(scalarProduct3) >= almostZero)
isOrthonormal = false;
if (!id.IsIdentity(1.0))
isOrthonormal = false;
cout << "sp 1 " << scalarProduct1 << " sp 2 " << scalarProduct2 << " sp 3 "
<< scalarProduct3 << " orthonormal columns " << isOrthonormal
<< endl;
cout << "p1 est " << P1Comp << endl;
cout << "p2 est " << P2Comp << endl;
for (int i = 0; i < numPoints; i++) {
cout << "point " << i << " triangulation "
<<
triangulator.
Triangulate(P1Comp, P2Comp, p1[i], p2[i], tempPoint)
<< " ";
}
cout << endl;
for (int i = 0; i < numPointsOnLines; i++) {
triangulator.
Triangulate(P1Comp, P2Comp, l1P1[i], l1P2[i],
tempPoint);
triangulator.
Triangulate(P1Comp, P2Comp, l2P1[i], l2P2[i],
tempPoint);
}
for (int i = 0; i < numPoints; i++) {
}
for (int i = 0; i < numPoints; i++) {
}
for (int i = 0; i < numPointsOnLines; i++) {
}
for (int i = 0; i < numPointsOnLines; i++) {
}
threeDOut.
AddPMatrix(P1, 512, 512, RGBAuc_WHITE_OPAQUE, 0.1);
return 0;
}