1. 引言
代码实战
//
// 本程序演示如何从Essential矩阵计算R,t
//
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
#include <sophus/so3.hpp>
#include <iostream>
using namespace std;
int main(int argc, char **argv) {
// 给定Essential矩阵
Matrix3d E;
E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
-0.006788487241438284, -0.5815434272915686, -0.01438258684486258;
// 待计算的R,t
Matrix3d R;
Vector3d t;
// SVD and fix sigular values
// START YOUR CODE HERE
//E = U * S * VT
Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, ComputeThinU | ComputeThinV);//初始化jacabisvd对象
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
Eigen::VectorXd singular_b = svd.singularValues();
Eigen::Matrix3d S1 = U.inverse() * E * V.transpose().inverse();
cout << "E = \n" << E << endl <<endl;
cout << "U*S*VT = \n" << U * S1 * V.transpose() << endl << endl;
cout << "singular value = \n" << singular_b << endl;
cout << "U = \n" << U << endl <<endl;
cout << "S1 = \n" << S1 << endl <<endl;
cout << "v = \n" << U << endl <<endl;
Eigen::Vector3d V2 = {(singular_b[0] + singular_b[1]) / 2, (singular_b[0] + singular_b[1]) /
2, 0};
Eigen::Matrix3d S =V2.asDiagonal();
cout << "S update = \n" << S << endl <<endl;
// END YOUR CODE HERE
// set t1, t2, R1, R2
// START YOUR CODE HERE
Matrix3d t_wedge1;
Matrix3d t_wedge2;
Matrix3d R1;
Matrix3d R2;
// END YOUR CODE HERE
cout << "R1 = " << R1 << endl;
cout << "R2 = " << R2 << endl;
cout << "t1 = " << Sophus::SO3d::vee(t_wedge1) << endl;
cout << "t2 = " << Sophus::SO3d::vee(t_wedge2) << endl;
// check t^R=E up to scale
Matrix3d tR = t_wedge1 * R1;
cout << "t^R = " << tR << endl;
return 0;
}
结果显示: