(01)ORB-SLAM2源码无死角解析-(58) 闭环线程→计算Sim3: 源码Sim3Solver::iterate()讲解

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

前面的博客，已经对 src/LoopClosing.cc 文件中 ComputeSim3() 函数的整体流程进行了讲解， 以及 Sim3 相关理论推导。关于 Sim3 的计算，主要推导出了如下几个公式： ( 1 ) ： \color{blue}(1)： (1)：先计算旋转矩阵 R \mathbf{R} R，首先构建： M = ∑ i = 1 n P i ′ Q i T = [ S x x S x y S x z S y x S y y S y z S z x S z y S z z ] (01) \color{Green} \tag{01} \begin{aligned} M &=\sum_{i=1}^{n} P_{i}^{\prime} Q_{i}^{T} =\left[\begin{array}{lll} S_{x x} & S_{x y} & S_{x z} \\ S_{y x} & S_{y y} & S_{y z} \\ S_{z x} & S_{z y} & S_{z z} \end{array}\right] \end{aligned} M​=i=1∑n​Pi′​QiT​=⎣ ⎡​Sxx​Syx​Szx​​Sxy​Syy​Szy​​Sxz​Syz​Szz​​⎦ ⎤​​(01)然后得到矩阵 N N N，对 N N N 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，再把该四元数转换成旋转矩阵 R \mathbf{R} R。   ( 2 ) ： \color{blue}(2)： (2)：有两方式来计算尺度 s s s，第一种是非对称性尺度，依赖转矩阵 R \mathbf{R} R(ORBSLAM2使用)；第二种是对称性尺度，不依赖旋转矩阵(推荐)；其计算方式分别如下： s = D S P = ∑ i = 1 n Q i ′ R P i ′ ∑ i = 1 n ∥ P i ′ ∥ 2                             s = S Q S P = ∑ i = 1 n ∥ Q i ′ ∥ 2 ∑ i = 1 n ∥ P i ′ ∥ 2 (02) \color{Green} \tag{02} s=\frac{D}{S_{P}}=\frac{\sum_{i=1}^{n} Q_{i}^{\prime} \mathbf R P_{i}^{\prime}}{\sum_{i=1}^{n}\left\|P_{i}^{\prime}\right\|^{2}}~~~~~~~~~~~~~~~~~~~~~~~~~~~s=\sqrt{\frac{S_{Q}}{S_{P}}}=\sqrt{\frac{\sum_{i=1}^{n}\left\|Q_{i}^{\prime}\right\|^{2}}{\sum_{i=1}^{n}\left\|P_{i}^{\prime}\right\|^{2}}} s=SP​D​=∑i=1n​∥Pi′​∥2∑i=1n​Qi′​RPi′​​                           s=SP​SQ​​ ​=∑i=1n​∥Pi′​∥2∑i=1n​∥Qi′​∥2​ ​(02)

( 3 ) ： \color{blue}(3)： (3)：有了旋转矩阵 R \mathbf{R} R 以及 尺度 s s s之后，则进一步可以求出平移 t \mathbf{t} t

t ∗ = Q ˉ − s R P ˉ (03) \color{Green} \tag{03} \mathbf t^{*}=\bar{Q}-s \mathbf R \bar{P} t∗=Qˉ​−sRPˉ(03)

下面就是需要分析源码，与上面的公式一一对应起来，首先来看一下 ComputeSim3() 中涉及到计算 Sim3 的函数代码：

	Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
	
	// Sim3Solver Ransac 过程置信度0.99，至少20个inliers 最多300次迭代
	pSolver->SetRansacParameters(0.99,20,300);

    // 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
    cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

其上最重要的代码为 pSolver->iterate(5,bNoMore,vbInliers,nInliers) 也是接下来重点介绍的代码。  

二、整理流程

Sim3Solver::iterate() 函数位于 src/Sim3Solver.cc 文件中，整体流程如下。

( 1 ) ： \color{blue}(1)： (1)： 进入循环，循环需要满足两个条件：①已经进行的总迭代次数还没有超过限制的最大总迭代次数mRansacMaxIts，当前迭代次数还没有超过理论迭代次数nIterations(默认为5)

( 2 ) ： \color{blue}(2)： (2)：通过前面的博客，已经推导出: 如果知道两个相机坐标系下3组不共线的匹配三位点，即可计算出他们的 Sim3 矩阵。所以每次循环都随机挑选出(不重复)匹配的3对地图点，分别为 P3Dc2i，P3Dc2i。

( 3 ) ： \color{blue}(3)： (3)：根据随机取的两组匹配的3D点，计算P3Dc2i 到 P3Dc1i 的Sim3变换

( 4 ) ： \color{blue}(4)： (4)：对计算的Sim3变换，通过投影误差进行inlier检测。简单的说，就是循环遍历匹配3D点→①把2系中的3D经过Sim3变换(mT12i)到1系中计算重投影坐标。②把1系中的3D经过Sim3变换(mT21i)到2系中计算重投影坐标。如果重投影误差都在设定范围之内，则认为该3对匹配点计算出来的 Sim3 合理，并且 mvbInliersi 标记为 true。

( 5 ) ： \color{blue}(5)： (5)：记录并更新最多的内点数目及对应的参数，只要计算得到一次合格的Sim变换，就直接成功返回，内点数mnInliersi>mRansacMinInliers(默认为20)则认为合格。如果已经达到了最大迭代次数了还没得到满足条件的Sim3，说明失败了，放弃，返回。

代码注释如下：

/**
 * @brief Ransac求解mvX3Dc1和mvX3Dc2之间Sim3，函数返回mvX3Dc2到mvX3Dc1的Sim3变换
 * 
 * @param[in] nIterations           设置的最大迭代次数
 * @param[in] bNoMore               为true表示穷尽迭代还没有找到好的结果，说明求解失败
 * @param[in] vbInliers             标记是否是内点
 * @param[in] nInliers              内点数目
 * @return cv::Mat                  计算得到的Sim3矩阵
 */
cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers)
{
    bNoMore = false;                        // 现在还没有达到最好的效果
    vbInliers = vector(mN1,false);    // 的确和最初传递给这个解算器的地图点向量是保持一致
    nInliers=0;                             // 存储迭代过程中得到的内点个数

    // Step 1 如果匹配点比要求的最少内点数还少，不满足Sim3 求解条件，返回空
    // mRansacMinInliers 表示RANSAC所需要的最少内点数目
    if(N