Aug 09 2012

Decomposing the Essential matrix using Horn and Eigen [w/code]

Published by at 1:56 am under code,opencv,vision

Hi

I've been working feverishly to straighten up the Structure from Motion Toy Library, and make it more robust. During my experiments with different methods I wanted to test out a different method for decomposing the Essential matrix to rotation R and translation t, other than that of Hartley and Zisserman using SVD. That's when I came upon this paper: here by Berthold Horn from 1990, that traces the steps of Longuet-Higgins who came up with the derivation for the Essential matrix. It has a closed form solution that works pretty well, and here it is implemented with the Eigen math library (a very good library to get to know).

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
	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(2));
	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)
	R2 = (cofactors.transpose() - B1*E) / b1.dot(b1);
	R1 = (cofactors.transpose() - B2*E) / b2.dot(b2);
	Map<Vector3d> t1(_t1),t2(_t2); 
	t1 = b2; t2 = b1;
	
	cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
}

Once I get the rest of the Structure from Motion library up to a certain level, I will make a more thorough post.

Enjoy
Roy.

Share

3 responses so far

3 Responses to “Decomposing the Essential matrix using Horn and Eigen [w/code]”

  1. Brunoon 09 Aug 2012 at 3:23 pm

    Very nice and useful post! Out of curiosity, do you have any special reasons to avoid the Hartley/Zisserman SVD method?

  2. Royon 09 Aug 2012 at 7:18 pm

    @Bruno
    Actually I don't have a clear reason, it's simply another method to try.. In most cases both methods produce almost exactly the same results.

  3. cdcon 11 Feb 2013 at 11:25 am

    I am currently faced with a problem regarding the E decomposition: neither of the 4 solution is valid (for all of them half of the points are in front and half behind). This happens only for some rare photo-sets. Do you guys know a solution?

Trackback URI | Comments RSS

Leave a Reply