从基础矩阵中求解本质矩阵，得到相机的参数（三维重建task1-5）

从基础矩阵中求解本质矩阵，得到相机的参数（三维重建task1-5）

代码呈上

#include <math/matrix.h>
#include <math/matrix_svd.h>

typedef math::Matrix<double, 3, 3> FundamentalMatrix;
typedef math::Matrix<double, 3, 3> EssentialMatrix;

//用于测试相机姿态的正确性
math::Vec2d p1={
   0.18012331426143646, -0.15658402442932129};
math::Vec2d p2={
   0.2082643061876297, -0.035404585301876068};
/*第一个相机的内外参数*/
double f1 = 0.972222208;
/*第二个相机的内外参数*/
double f2 = 0.972222208;


/**
 * \description 对匹配点进行三角化得到空间三维点
 * @param p1 -- 第一幅图像中的特征点
 * @param p2 -- 第二幅图像中的特征点
 * @param K1 -- 第一幅图像的内参数矩阵
 * @param R1 -- 第一幅图像的旋转矩阵
 * @param t1 -- 第一幅图像的平移向量
 * @param K2 -- 第二幅图像的内参数矩阵
 * @param R2 -- 第二幅图像的旋转矩阵
 * @param t2 -- 第二幅图像的平移向量
 * @return 三维点
 */
math::Vec3d triangulation(math::Vec2d const & p1
        , math::Vec2d const & p2
        , math::Matrix3d const &K1
        , math::Matrix3d const &R1
        , math::Vec3d const & t1
        , math::Matrix3d const &K2
        , math::Matrix3d const &R2
        , math::Vec3d const& t2){
   


    // 构造投影矩阵
    math::Matrix<double, 3, 4>P1, P2;

    math::Matrix<double, 3, 3> KR1 = K1 * R1;
    math::Matrix<double, 3, 1> Kt1(*(K1 * t1));
    P1 = KR1.hstack(Kt1);

    math::Matrix<double, 3, 3> KR2 = K2 * R2;
    math::Matrix<double, 3, 1> Kt2(*(K2 * t2));
    P2 = KR2.hstack(Kt2);

    std::cout<<"P1: "<<P1<<std::endl;
    std::cout<<"P1 for fist pose should be\n"
             <<"0.972222 0 0 0\n"
             <<"0 0.972222 0 0\n"
             <<"0 0 1 0\n";

    std::cout<<"P2: "<<P2<<std::endl;
    std::cout<<"P2 for fist pose should be\n"
             <<" -0.957966 0.165734 -0.00707496 0.0774496\n"
             <<"0.164089 0.952816 0.102143 0.967341\n"
             <<"0.0250416 0.102292 -0.994439 0.0605768\n";

    /* 构造A矩阵 */
    math::Matrix<double, 4, 4> A;
    // 对A的每一列分别进行赋值
    for(int i=0; i<4; i++){
   
        // 第1个点
        A(0, i) = p1