void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
using namespace Eigen;
Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
Matrix3d EEt = E * E.transpose();
Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(0));
Vector3d b1,b2;
#if 1
//Method 1
Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
Vector3d bbt_diag = bbt.diagonal();
if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
b1 = bbt.row(0) / sqrt(bbt_diag(0));
b2 = -b1;
} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
b1 = bbt.row(1) / sqrt(bbt_diag(1));
b2 = -b1;
} else {
b1 = bbt.row(2) / sqrt(bbt_diag(2));
b2 = -b1;
}
#else
//Method 2
if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else {
b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
}
#endif
//Horn90 (19)
Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
cofactors.transposeInPlace();
//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
Matrix3d B1; B1 << 0,-b1(2),b1(1),
b1(2),0,-b1(0),
-b1(1),b1(0),0;
Matrix3d B2; B2 << 0,-b2(2),b2(1),
b2(2),0,-b2(0),
-b2(1),b2(0),0;
Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);
//Horn90 (24)
R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
Map<Vector3d> t1(_t1),t2(_t2);
t1 = b1; t2 = b2;
cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}
void CFullRelPose::writeEdge(std::ostream & toroGraphFile, const bool b2d) const
{
if(IS_DEBUG) CHECK(!hasPosition(), "Node is disconnected?? Or too bad");
const double dLength = length();
//double dVar = sqr(dLength/4);// scale.scaleVarHacked();
double dVar = variance();
dVar = std::max<double>(1e-5, dVar);
normalisedPose.scale(dLength).write(toroGraphFile, b2d);
Vector3d t, x_axis; x_axis.setZero();
normalisedPose.t.asVector(t);
if(b2d)
{
//double inf_ff, inf_fs, inf_ss, inf_rr, inf_fr, inf_sr;
Vector2d t2d(t(0), t(2));
Vector2d t2d_perp(t(2), -t(0));
Matrix2d Q, LAMBDA;
LAMBDA.setZero();
Q << t2d, t2d_perp;
LAMBDA(0,0) = dVar; //dVar may be zero
LAMBDA(1,1) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());
CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")
const Matrix2d C = Q * LAMBDA * Q.transpose();
const Matrix2d & I = C.inverse();
//cout << I << endl;
//THROW("Not complete...")
double dRotInf = 1.0/sqr(normalisedPose.SD.relOrientationSD());
toroGraphFile << I(0,0) << ' ' << I(1,0) << ' ' << I(1,1) << ' ' << dRotInf << " 0 0";
return;
}
if(t(0) < 0.9) //check t isn't x axis aligned (x is arbitrary)
x_axis(0) = 1;
else
x_axis(1) = 1;
Matrix3d Q;
if(t.sum() == 0) //Pure rotation. Should be arbitrary
{
Q.setIdentity();
}
else
{
Vector3d tperp1 = t.cross(x_axis);
tperp1.normalize();
Vector3d tperp2 = tperp1.cross(t);
Q << t, tperp1, tperp2;
}
CHECK(isnan(Q.sum()), "writeEdge: nan")
Matrix3d LAMBDA; LAMBDA.setZero();
LAMBDA(0,0) = dVar; //dVar may be zero
LAMBDA(1,1) = LAMBDA(2,2) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());
CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")
const Matrix3d C = Q * LAMBDA * Q.transpose();
//std::cout << "Covariance: " << std::endl << C << std::endl;
CHECK(isnan(C.sum()), "writeEdge: nan")
Matrix3d Crpy; Crpy.setZero();
Crpy.diagonal().setConstant(sqr(normalisedPose.SD.relOrientationSD()));
Matrix<double, 6, 6> Cfull; Cfull << C, Matrix3d::Zero(), Matrix3d::Zero(), Crpy;
CHECK(isnan(Cfull.sum()), "writeEdge: nan")
if(Cfull.trace() < 0.0001)
{
std::cout << "Warning: Covariance matrix near-singular, adjusting diagonal\n";
Cfull.diagonal().array() += 0.0001;
}
const Matrix<double, 6, 6> & Ifull = Cfull.inverse();
//std::cout << "Information: " << std::endl << Ifull << std::endl;
//cout << "Warning: Not inverting information matrix\n"; Yes the information mat does work a little better.
//Possibly ok for 1 edge...if(IS_DEBUG) CHECK(Cfull.determinant()<0.0001, "CFullRelPose::writeEdge: Singular covariance matrix");
CHECK(isnan(Ifull.sum()), "writeEdge: nan")
for (int nRow = 0; nRow <6; nRow++)
{
for(int nCol = nRow; nCol < 6; nCol++)
toroGraphFile << Ifull(nRow, nCol) << ' ';
}
}
