ROS-3DSLAM（9）：视觉部分visual estimator第三节initial 3

2021@SDUSC

2021年11月14日星期日——2021年11月15日星期一

一、背景简介：

这一次继续分析initial文件夹下的内容，本次针对的是solve_5pts，包括了该文件的头文件和cpp文件。

这里的solve_5pts的意思是利用五点法求本质矩阵，代码则正是对于这一功能的实现。

在进行代码分析之前，有必要先学习了解一下五点法。

在slam 和sfm领域，恢复相机位姿和3D点的坐标是其重要的任务，描述一个场景的3D点在不同相机的图像坐标之间的关系被称为对极几何关系。对极几何关系描述的矩阵通常有基本矩阵(fundamental matrix)、本质矩阵（essential matrix）、单应矩阵(homography matrix)。基本矩阵的求解算法有7点法、8点法；基本矩阵的求解算法有5点法、8点法；单应矩阵的求解算法为DLT算法。

在对极几何关系矩阵中，单应矩阵的求解、基本矩阵和本质矩阵的8点法求解算法统称为线性求解，而基本矩阵的7点法和本质矩阵的5点法称为非线性求解，尤其本质矩阵的5点法求解过程非常复杂，大多数教材都是先计算基本矩阵，然后利用内参和基本矩阵求解本质矩阵(如orb2-slam中本质矩阵的求解)，5点法的求解涉及10次多项式的求解，故相关资料较少(包括波恩大学photogrammetry 课程，5点法求解E矩阵的过程也是直接跳过)。

可以看出来这里的五点法求本质矩阵的过程十分复杂，但是如果想要分析代码的话，对于算法又不得不学习，为此，我决定先浏览一下下基本的算法流程，然后再根据代码进行进一步的分析。

---

在阅读的过程中我发现，原来这个文件的重点并不是实现五点法的计算，而是利用

我们保持了一个帧的滑动窗口来限制计算复杂度。首先，我们检查了最新帧与之前所有帧之间的特征对应。如果我们能找到稳定的特征跟踪(超过30个跟踪特征)和足够的视差(20个以上的旋转补偿像素)在滑动窗口中的最新帧和任何其他帧之间，我们恢复相对旋转和这两个帧之间的上尺度平移使用五点算法。

五点算法来得到尺度，因此这里的recoverPose函数是重点。

在处理相关帧的关系solveRelativeRT时，计算得到本质矩阵，传给recoverPose进行变换，其间可以利用decomposeE来进行分解。

testTrangulation的作用我在上一篇分析中提到过，但是还是不是很确定功能，存有疑问，为什么要这样安排结构也让我不是很理解。

总之，这个文件的功能就是利用五点算法恢复相对旋转和两个帧之间上的尺度。

X、五点法算法：

P 设置矩阵表达式

A 提取特征向量

B 加入约束方程

C 利用高斯消元法

E 求解本质矩阵

二、代码分析：

1、头文件

没有定义数据结构，只是给出了三个函数的声明，根据英文名称猜测，分别是“处理相关帧关系”、“测试三角化”、“分解本质矩阵”。

其中，decomposeE和上一篇[initial_ex_rotation]当中的算法一模一样，这里又算了一遍？

这里设置成private是为了什么呢？

class MotionEstimator
{
   
  public:

    bool solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &R, Vector3d &T);

  private:
    double testTriangulation(const vector<cv::Point2f> &l,
                             const vector<cv::Point2f> &r,
                             cv::Mat_<double> R, cv::Mat_<double> t);
    void decomposeE(cv::Mat E,
                    cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                    cv::Mat_<double> &t1, cv::Mat_<double> &t2);
};

2、solve_5pts

#include "solve_5pts.h"


namespace cv {
   
    //分解本质矩阵
    void decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t )
    {
   
        Mat E = _E.getMat().reshape(1, 3);
        CV_Assert(E.cols == 3 && E.rows == 3);

        Mat D, U, Vt;
        //利用了svd分解法
        SVD::compute(E, D, U, Vt);

        if (determinant(U) < 0) U *= -1.;
        if (determinant(Vt) < 0) Vt *= -1.;

        Mat W = (Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
        W.convertTo(W, E.type());

        Mat R1, R2, t;
        R1 = U * W * Vt;
        R2 = U * W.t() * Vt;
        t = U.col(2) * 1.0;

        R1.copyTo(_R1);
        R2.copyTo(_R2);
        t.copyTo(_t);
    }
	//恢复位姿，注意这里的输入分别是本质矩阵、第一幅图像、第二幅图像、相机内参、第一帧到第二帧的旋转矩阵、第一帧到第二帧的平移向量、标记没有舍弃的点
    int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
                         OutputArray _R, OutputArray _t, InputOutputArray _mask)
    {
   
		//传入的参数改变格式
        Mat points1, points2, cameraMatrix;
        _points1.getMat().convertTo(points1, CV_64F);
        _points2.getMat().convertTo(points2, CV_64F);
        _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);

        int npoints = points1.checkVector(2);
        CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
                                  points1.type() == points2.type());

        CV_Assert