验证本质矩阵E的 分解

#include <iostream>
#include <vector>
#include <Eigen/Eigen>
#include <Eigen/Dense>

int main() {
    //SHAPE_FACTORY(BoxCude) < ClipFunType > ss;
    //std::cout << int(ss.points_.size()) << "\n";
    //std::cout << int(ss.shape_type_) << "\n";

    Eigen::Matrix3f R;
    R = Eigen::AngleAxisf(3.1415926 / 2.0, Eigen::Vector3f::UnitZ()) *
        Eigen::AngleAxisf(3.1415926 / 4.0, Eigen::Vector3f::UnitX()) *
        Eigen::AngleAxisf(3.1415926 / 4.0, Eigen::Vector3f::UnitY());
    Eigen::Vector3f t(0.2, 3.0, 1.2);
    Eigen::Matrix3f t_x;
    t_x << 0.0, -t[2], t[1], 
        t[2], 0.0, -t[0], 
        -t[1], t[0], 0.0;

    Eigen::Matrix3f essential_matrix_t = t_x * R;
    Eigen::BDCSVD<Eigen::MatrixXf> svd_e(essential_matrix_t, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::VectorXf sigma = svd_e.singularValues();
    Eigen::MatrixXf U = svd_e.matrixU();
    Eigen::MatrixXf V = svd_e.matrixV();

    Eigen::Matrix3f diag_sigma;
    diag_sigma.setZero();
    if (sigma.size() >= 2)
    {
        std::cout << "sigma: " << sigma[0] << " " << sigma[1] << " " << sigma[2] << "\n";
        diag_sigma(0, 0) = sigma[0];
        diag_sigma(1, 1) = sigma[1];
    }

    Eigen::Matrix3f R_2pi, R_2pi_f;
    R_2pi = Eigen::AngleAxisf(3.1415926 / 2.0, Eigen::Vector3f::UnitZ()) *
        Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitX()) *
        Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitY());
    R_2pi_f = Eigen::AngleAxisf(-3.1415926 / 2.0, Eigen::Vector3f::UnitZ()) *
        Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitX()) *
        Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitY());

    Eigen::Matrix3f R1, R2;
    Eigen::Vector3f t1, t2;
    R1 = U * R_2pi.transpose() * V.transpose();
    R2 = U * R_2pi_f.transpose() * V.transpose();

    Eigen::Matrix3f t1_x = U * R_2pi * diag_sigma * U.transpose();
    Eigen::Matrix3f t2_x = U * R_2pi_f * diag_sigma * U.transpose();

    std::cout << "t1_x : \n" << t1_x << "\n ";
    std::cout << "t2_x : \n" << t2_x << "\n ";

    return 0;
}