opencv矩阵转eigen_对极几何的基础矩阵和本质矩阵之实战

系列篇：

对极几何的基础矩阵和本质矩阵1

对极几何的基础矩阵和本质矩阵2

对极几何的基础矩阵和本质矩阵3

前面几篇文章在理论上对基础矩阵和本质矩阵进行了推导和运算，为了更好的理解，本文着重在代码的演示部分，对于已经很熟练的，可以不予关注本篇。

定义头文件 stereo.h

#ifndef SLAM_ALGORITHM_STEREO_H#define SLAM_ALGORITHM_STEREO_H#include #include #include #include namespace SLAM_STEREO{
    // 向量的叉乘Eigen::Matrix3d corss(Eigen::Vector3d& t);// 提取棋盘格角点bool findCorners(cv::Mat& img, cv::Size& patternSize, std::vector<:point2f> &corners,cv::Mat& drawMat);// 三角化void triangulatePoint(Eigen::Matrix<double, 3, 4>& T1, Eigen::Matrix<double, 3, 4>& T2, Eigen::Vector3d& p1, Eigen::Vector3d&p2, Eigen::Vector3d& out);// 4中姿态中，选择两个相机之前的。bool choose_pose(Eigen::Matrix3d& K1, Eigen::Matrix3d& R1, Eigen::Vector3d& t1,        Eigen::Matrix3d& K2,  Eigen::Matrix3d& R2, Eigen::Vector3d& t2,        Eigen::Vector3d& p1, Eigen::Vector3d& p2);// 像素坐标转相机的归一化坐标Eigen::Vector3d  pix2cam(Eigen::Matrix3d& K, Eigen::Vector3d& pix);// dlt 求解 基础矩阵void find_fundamental_dlt(std::vector<:vector3d>& imagePoints1,        std::vector<:vector3d>& imagePoints2, Eigen::Matrix3d& F);// dlt 求解 本质矩阵void find_essential_dlt(std::vector<:vector3d>& camPoints1,                        std::vector<:vector3d>& camPoints2, Eigen::Matrix3d& E);// 根据F矩阵，求解E矩阵void find_essential_from_fundamental(Eigen::Matrix3d& K1,Eigen::Matrix3d& K2, Eigen::Matrix3d& F, Eigen::Matrix3d& E);// E 矩阵分解void decompose_from_essential(Eigen::Matrix3d& E, Eigen::Matrix3d& R1,Eigen::Matrix3d& R2,  Eigen::Vector3d& t1, Eigen::Vector3d& t2);}#endif //SLAM_ALGORITHM_STEREO_H

我这里重点贴一下dlt 求解F矩阵，E矩阵和E矩阵分解姿态。

dlt 求解F矩阵：为了运算方便，像素坐标是齐次坐标的形式

void find_fundamental_dlt(std::vector<:vector3d>& imagePoints1,                              std::vector<:vector3d>& imagePoints2, Eigen::Matrix3d& F){
            assert(imagePoints2.size()==imagePoints1.size());        using namespace Eigen;        int N = imagePoints1.size();        MatrixXd A(N, 9);        for (int i = 0; i             Vector3d& p1 =imagePoints1.at(i);            Vector3d& p2 =imagePoints2.at(i);            A(i, 0) = p1[0]*p2[0];            A(i, 1) = p1[1]*p2[0];            A(i, 2) = p2[0];            A(i, 3) = p1[0]*p2[1];            A(i, 4) = p1[1]*p2[1];            A(i, 5) = p2[1];            A(i, 6) = p1[0];            A(i, 7) = p1[1];            A(i, 8) = 1.0;        }        // SVD 分解        Eige