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

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

前面的博客，已经对 src/LoopClosing.cc 文件中 ComputeSim3() 函数的整体流程进行了讲解，以及 Sim3 相关理论推导。关于 Sim3 的计算，主要推导出了如下几个公式：
$\color{blue}(1)：$ 先计算旋转矩阵 $\mathbf{R}$ ，首先构建： $\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}$ 然后得到矩阵 $N$ ，对 $N$ 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，再把该四元数转换成旋转矩阵 $\mathbf{R}$ 。

$\color{blue}(2)：$ 有两方式来计算尺度 $s$ ，第一种是非对称性尺度，依赖转矩阵 $\mathbf{R}$ (ORBSLAM2使用)；第二种是对称性尺度，不依赖旋转矩阵(推荐)；其计算方式分别如下：
$\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}}}$

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

$\color{Green} \tag{03} \mathbf t^{*}=\bar{Q}-s \mathbf R \bar{P}$

下面就是需要分析源码，与上面的公式一一对应起来，首先来看一下 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 文件中，整体流程如下。

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

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

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

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

$\color{blue}(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<bool> &vbInliers, int &nInliers)
{
    
    
    bNoMore = false;                        // 现在还没有达到最好的效果
    vbInliers = vector<bool>(mN1,false);    // 的确和最初传递给这个解算器的地图点向量是保持一致
    nInliers=0;                             // 存储迭代过程中得到的内点个数

    // Step 1 如果匹配点比要求的最少内点数还少，不满足Sim3 求解条件，返回空
    // mRansacMinInliers 表示RANSAC所需要的最少内点数目
    if(N<mRansacMinInliers)
    {
    
    
        bNoMore = true;  // 表示求解失败
        return cv::Mat();   
    }

    // 可以使用的点对的索引,为了避免重复使用
    vector<size_t> vAvailableIndices;

    // 随机选择的来自于这两个帧的三对匹配点
    cv::Mat P3Dc1i(3,3,CV_32F);
    cv::Mat P3Dc2i(3,3,CV_32F);

    // nCurrentIterations：     当前迭代的次数
    // nIterations：            理论迭代次数
    // mnIterations：           总迭代次数
    // mRansacMaxIts：          最大迭代次数
    int nCurrentIterations = 0;
    // Step 2 随机选择三个点，用于求解后面的Sim3
    // 条件1: 已经进行的总迭代次数还没有超过限制的最大总迭代次数
    // 条件2: 当前迭代次数还没有超过理论迭代次数
    while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations)
    {
    
    
        nCurrentIterations++;// 这个函数中迭代的次数
        mnIterations++;      // 总的迭代次数，默认为最大为300

        // 记录所有有效（可以采样）的候选三维点索引
        vAvailableIndices = mvAllIndices;

        // Get min set of points
        // Step 2.1 随机取三组点，取完后从候选索引中删掉
        for(short i = 0; i < 3; ++i)
        {
    
    
            // DBoW3中的随机数生成函数
            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);

            int idx = vAvailableIndices[randi];

            // P3Dc1i和P3Dc2i中点的排列顺序：
            // x1 x2 x3 ...
            // y1 y2 y3 ...
            // z1 z2 z3 ...
            mvX3Dc1[idx].copyTo(P3Dc1i.col(i));
            mvX3Dc2[idx].copyTo(P3Dc2i.col(i));

            // 从"可用索引列表"中删除这个点的索引 
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }

        // Step 2.2 根据随机取的两组匹配的3D点，计算P3Dc2i 到 P3Dc1i 的Sim3变换
        ComputeSim3(P3Dc1i,P3Dc2i);

        // Step 2.3 对计算的Sim3变换，通过投影误差进行inlier检测
        CheckInliers();

        // Step 2.4 记录并更新最多的内点数目及对应的参数
        if(mnInliersi>=mnBestInliers)
        {
    
    
            mvbBestInliers = mvbInliersi;
            mnBestInliers = mnInliersi;
            mBestT12 = mT12i.clone();
            mBestRotation = mR12i.clone();
            mBestTranslation = mt12i.clone();
            mBestScale = ms12i;

            if(mnInliersi>mRansacMinInliers) // 只要计算得到一次合格的Sim变换，就直接返回
            {
    
    
                // 返回值,告知得到的内点数目
                nInliers = mnInliersi;
                for(int i=0; i<N; i++)
                    if(mvbInliersi[i])
                        // 标记为内点
                        vbInliers[mvnIndices1[i]] = true;
                return mBestT12;
            } // 如果当前次迭代已经合格了,直接返回
        } // 更新最多的内点数目
    } // 迭代循环

    // Step 3 如果已经达到了最大迭代次数了还没得到满足条件的Sim3，说明失败了，放弃，返回
    if(mnIterations>=mRansacMaxIts)
        bNoMore=true;

    return cv::Mat();   // no more的时候返回的是一个空矩阵
}

三、细节分析

通过上面的讲解与注释，可以很明显的看出来，最核心的函数为 ComputeSim3(P3Dc1i,P3Dc2i); 该函数实现于 src/Sim3Solver.cc 文件。

$\color{blue}(1)：$ 首先计算3D点质心及去质心后的点，也就是对应前面博客推导的如下公式：
$\color{Green} \tag{01} \bar{P}=\frac{1}{n} \sum_{i=1}^{n} P_{i}~~~~~~~~~~~~~~~~~~~~~\bar{Q}=\frac{1}{n} \sum_{i=1}^{n} Q_{i}$ $\color{Green} \tag{02} P_{i}^{\prime}=P_{i}-\bar{P}~~~~~~~~~~~~~~~~~~~~~~Q_{i}^{\prime}=Q_{i}-\bar{Q}$

$\color{blue}(2)：$ 计算 $M$ 与 $N$ :
$\color{Green} \tag{03} \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}$ $\color{Green} \tag{04} \begin{aligned} N &=\sum_{i=1}^{n} \mathbb{R}_{\mathbb{Q}, \mathrm{i}}^{\mathbb{T}} \overline{\mathbb{R}_{\mathbb{P}, \mathrm{i}}} \\ &=\left[\begin{array}{cccc} \left(S_{x x}+S_{y y}+S_{z z}\right) & S_{y z}-S_{z y} & S_{z x}-S_{x z} & S_{x y}-S_{y x} \\ S_{y z}-S_{z y} & \left(S_{x x}-S_{y y}-S_{z z}\right) & S_{x y}+S_{y x} & S_{z x}+S_{x z} \\ S_{z x}-S_{x z} & S_{x y}+S_{y x} & \left(-S_{x x}+S_{y y}-S_{z z}\right) & S_{y z}+S_{z y} \\ S_{x y}-S_{y x} & S_{z x}+S_{x z} & S_{y z}+S_{z y} & \left(-S_{x x}-S_{y y}+S_{z z}\right) \end{array}\right] \end{aligned}$

$\color{blue}(3)：$ 调用 cv::eigen(N,eval,evec) 函数计算对称矩阵特征值和特征向量，特征值默认是从大到小排列，所以evec[0] 是最大值。

$\color{blue}(4)：$ 先四元数转换成旋转向量 vec，然后再调用cv::Rodrigues(vec,mR12i)函数，即可以把旋转向量转换成旋转矩阵。
$\color{Green} \tag{05} \begin{array}{l} \theta=2 \arccos w ~~~~~~~~~~~~~~~~~~~~~\text {vec}= {\left[n_{x}, n_{y}, n_{z}\right]^{T}=\left[x, y, z\right]^{T} / \sin \frac{\theta}{2}} \end{array}$

$\color{blue}(5)：$ 根据前面推导出来的公式，计算出尺度s，但是这里需要注意，针对的是单目相机，如果为深度或者双目相机，尺度默认为1： $\color{Green} \tag{06} 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}}$

$\color{blue}(6)：$ 根据公式求解出平移 $\mathbf t$ $\color{Green} \tag{07} \mathbf t=\bar{Q}-s \mathbf R \bar{P}$

代码注释如下，位于 src/Sim3Solver.cc 文件之中：

/**
 * @brief 根据两组匹配的3D点,计算P2到P1的Sim3变换
 * @param[in] P1    匹配的3D点(三个,每个的坐标都是列向量形式,三个点组成了3x3的矩阵)(当前关键帧)
 * @param[in] P2    匹配的3D点(闭环关键帧)
 */
void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
{
    
    
    // Sim3计算过程参考论文:
    // Horn 1987, Closed-form solution of absolute orientataion using unit quaternions

    // Step 1: 定义3D点质心及去质心后的点
    // O1和O2分别为P1和P2矩阵中3D点的质心
    // Pr1和Pr2为减去质心后的3D点
    cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)
    cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)
    cv::Mat O1(3,1,Pr1.type()); // Centroid of P1
    cv::Mat O2(3,1,Pr2.type()); // Centroid of P2

    ComputeCentroid(P1,Pr1,O1);
    ComputeCentroid(P2,Pr2,O2);

    // Step 2: 计算论文中三维点数目n>3的 M 矩阵。这里只使用了3个点
    // Pr2 对应论文中 r_l,i'，Pr1 对应论文中 r_r,i',计算的是P2到P1的Sim3，论文中是left 到 right的Sim3
    cv::Mat M = Pr2*Pr1.t();

    // Step 3: 计算论文中的 N 矩阵

    double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;

    cv::Mat N(4,4,P1.type());

    N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2);   // Sxx+Syy+Szz
    N12 = M.at<float>(1,2)-M.at<float>(2,1);                    // Syz-Szy
    N13 = M.at<float>(2,0)-M.at<float>(0,2);                    // Szx-Sxz
    N14 = M.at<float>(0,1)-M.at<float>(1,0);                    // ...
    N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2);
    N23 = M.at<float>(0,1)+M.at<float>(1,0);
    N24 = M.at<float>(2,0)+M.at<float>(0,2);
    N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2);
    N34 = M.at<float>(1,2)+M.at<float>(2,1);
    N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2);

    N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,
                                 N12, N22, N23, N24,
                                 N13, N23, N33, N34,
                                 N14, N24, N34, N44);


    // Step 4: 特征值分解求最大特征值对应的特征向量，就是我们要求的旋转四元数

    cv::Mat eval, evec;  // val vec
    // 特征值默认是从大到小排列，所以evec[0] 是最大值
    cv::eigen(N,eval,evec); 

    // N 矩阵最大特征值（第一个特征值）对应特征向量就是要求的四元数（q0 q1 q2 q3），其中q0 是实部
    // 将(q1 q2 q3)放入vec（四元数的虚部）
    cv::Mat vec(1,3,evec.type());
    (evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis)


    // Rotation angle. sin is the norm of the imaginary part, cos is the real part
    // 四元数虚部模长 norm(vec)=sin(theta/2), 四元数实部 evec.at<float>(0,0)=q0=cos(theta/2)
    // 这一步的ang实际是theta/2，theta 是旋转向量中旋转角度
    // ? 这里也可以用 arccos(q0)=angle/2 得到旋转角吧
    double ang=atan2(norm(vec),evec.at<float>(0,0));

    // vec/norm(vec)归一化得到归一化后的旋转向量,然后乘上角度得到包含了旋转轴和旋转角信息的旋转向量vec
    vec = 2*ang*vec/norm(vec); //Angle-axis x. quaternion angle is the half

    mR12i.create(3,3,P1.type());
    // 旋转向量（轴角）转换为旋转矩阵
    cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis

    // Step 5: Rotate set 2
    // 利用刚计算出来的旋转将三维点旋转到同一个坐标系，P3对应论文里的 r_l,i', Pr1 对应论文里的r_r,i'
    cv::Mat P3 = mR12i*Pr2;

    // Step 6: 计算尺度因子 Scale
    if(!mbFixScale)
    {
    
    
        // 论文中有2个求尺度方法。一个是p632右中的位置，考虑了尺度的对称性
        // 代码里实际使用的是另一种方法，这个公式对应着论文中p632左中位置的那个
        // Pr1 对应论文里的r_r,i',P3对应论文里的 r_l,i',(经过坐标系转换的Pr2), n=3, 剩下的就和论文中都一样了
        double nom = Pr1.dot(P3);
        // 准备计算分母
        cv::Mat aux_P3(P3.size(),P3.type());
        aux_P3=P3;
        // 先得到平方
        cv::pow(P3,2,aux_P3);
        double den = 0;

        // 然后再累加
        for(int i=0; i<aux_P3.rows; i++)
        {
    
    
            for(int j=0; j<aux_P3.cols; j++)
            {
    
    
                den+=aux_P3.at<float>(i,j);
            }
        }

        ms12i = nom/den;
    }
    else
        ms12i = 1.0f;

    // Step 7: 计算平移Translation
    mt12i.create(1,3,P1.type());
    // 论文中平移公式
    mt12i = O1 - ms12i*mR12i*O2;

    // Step 8: 计算双向变换矩阵，目的是在后面的检查的过程中能够进行双向的投影操作

    // Step 8.1 用尺度，旋转，平移构建变换矩阵 T12
    mT12i = cv::Mat::eye(4,4,P1.type());

    cv::Mat sR = ms12i*mR12i;

    //         |sR t|
    // mT12i = | 0 1|
    sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));
    mt12i.copyTo(mT12i.rowRange(0,3).col(3));

    // Step 8.2 T21

    mT21i = cv::Mat::eye(4,4,P1.type());

    cv::Mat sRinv = (1.0/ms12i)*mR12i.t();
    sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));
    cv::Mat tinv = -sRinv*mt12i;
    tinv.copyTo(mT21i.rowRange(0,3).col(3));
}

三、结语

通过一系列的讲解，可以说已经把 Sim3 变换的来龙去脉都弄清楚了，对于 src/LoopClosing 中的 LoopClosing ::ComputeSim3() 函数，主要有如下比较重要的函数：

int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

matcher.SearchByBoW 以及 matcher.SearchByProjection 通过前面的博客理解起来不难，这里就不做讲解了。SearchBySim3 函数逻辑比较简单，明白了 Sim3 变换之后，再去理解该函数还是比较简单的。至于 Optimizer::OptimizeSim3() 函数涉及到图优化，再后面有专门的章节进行统一的讲解。