视觉slam14讲学习（六）之单目视觉里程计实战

1. 引言

5点法求解R,t理论部分

代码实战

//
// 本程序演示如何从Essential矩阵计算R,t
//

#include 
#include 
#include 

using namespace Eigen;

#include 

#include 

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 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 <