从Essential Matrix估计R,T

本文主要是介绍从Essential Matrix估计R,T，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

clc
clear
t=rand(3,1)
R=rodrigues(rand(3,1))
T=[0 -t(3) t(2);t(3) 0 -t(1);-t(2) t(1) 0
];E=T*R
[U,S,V]=svd(E);
disp('S?=?diag(1,1,0)')
S
W=[0 -1 0;1 0 0;0 0 1
];
P1=[U*W*V' U(:,3)]
P2=[U*W'*V' U(:,3)]
disp('check R..')
norm(U*W*V'-R)
norm(U*W'*V'-R)disp('check t..')
norm(U(:,3) - t/norm(t))
norm(- U(:,3) - t/norm(t))

Decomposing the Essential matrix using Horn and Eigen 

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.pdfusing 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 1Matrix3d 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 2if (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_multiplicationMatrix3d 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;
}

http://www.morethantechnical.com/2012/08/09/decomposing-the-essential-matrix-using-horn-and-eigen-wcode/

http://www.cnblogs.com/cutepig/archive/2007/07/12/815351.html

这篇关于从Essential Matrix估计R,T的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---