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

1. 引言

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

代码实战

//
// 本程序演示如何从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;
}

结果显示: