(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
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

前面的博客，已经对 src/LoopClosing.cc 文件中 ComputeSim3() 函数的整体流程进行了讲解， 以及 Sim3 相关理论推导。关于 Sim3 的计算，主要推导出了如下几个公式：
$(1)：$先计算旋转矩阵 $\mathbf{R}$，首先构建：$$\begin{array}{rl}M& =\sum _{i=1}^n{P}_i^{\prime }{Q}_i^T=\left[\begin{array}{lll}{S}_{xx}& S_{xy}& S_{xz}\\ S_{yx}& S_{yy}& S_{yz}\\ S_{zx}& S_{zy}& S_{zz}\end{array}\right]\end{array}\text{\hspace{1em}(01)}$$然后得到矩阵 $N$，对 $N$ 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，再把该四元数转换成旋转矩阵 $\mathbf{R}$。
 
$(2)：$有两方式来计算尺度 $s$，第一种是非对称性尺度，依赖转矩阵 $\mathbf{R}$(ORBSLAM2使用)；第二种是对称性尺度，不依赖旋转矩阵(推荐)；其计算方式分别如下：
$$s=\frac{D}{S_P}=\frac{{\sum }_{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}s=\sqrt{\frac{S_Q}{S_P}}=\sqrt{\frac{{\sum }_{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}}\text{\hspace{1em}(02)}$$

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

$${\mathbf{t}}^{\ast }=\bar{Q}-s\mathbf{R}\bar{P}\text{\hspace{1em}(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);
1
2
3
4
5
6
7

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

二、整理流程

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

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

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

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

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

$(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的时候返回的是一个空矩阵
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

 

三、细节分析

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

$(1)：$ 首先计算3D点质心及去质心后的点，也就是对应前面博客推导的如下公式：
$$\bar{P}=\frac{1}{n}\sum _{i=1}^n{P}_i\bar{Q}=\frac{1}{n}\sum _{i=1}^n{Q}_i\text{\hspace{1em}(01)}$$$$P_i^{\prime }=P_i-\bar{P}{Q}_i^{\prime }=Q_i-\bar{Q}\text{\hspace{1em}(02)}$$

$(2)：$ 计算 $M$ 与 $N$:
$$\begin{array}{rl}M& =\sum _{i=1}^n{P}_i^{\prime }{Q}_i^T=\left[\begin{array}{lll}{S}_{xx}& S_{xy}& S_{xz}\\ S_{yx}& S_{yy}& S_{yz}\\ S_{zx}& S_{zy}& S_{zz}\end{array}\right]\end{array}\text{\hspace{1em}(03)}$$$$\begin{array}{rl}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_{xx}+S_{yy}+S_{zz}\right)& S_{yz}-S_{zy}& S_{zx}-S_{xz}& S_{xy}-S_{yx}\\ S_{yz}-S_{zy}& \left(S_{xx}-S_{yy}-S_{zz}\right)& S_{xy}+S_{yx}& S_{zx}+S_{xz}\\ S_{zx}-S_{xz}& S_{xy}+S_{yx}& \left(-S_{xx}+S_{yy}-S_{zz}\right)& S_{yz}+S_{zy}\\ S_{xy}-S_{yx}& S_{zx}+S_{xz}& S_{yz}+S_{zy}& \left(-S_{xx}-S_{yy}+S_{zz}\right)\end{array}\right]\end{array}\text{\hspace{1em}(04)}$$
 
$(3)：$ 调用 cv::eigen(N,eval,evec) 函数计算对称矩阵特征值和特征向量，特征值默认是从大到小排列，所以evec[0] 是最大值。
 
$(4)：$先四元数转换成旋转向量 vec，然后再调用cv::Rodrigues(vec,mR12i)函数，即可以把旋转向量转换成旋转矩阵。
$$\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}\text{\hspace{1em}(05)}$$

$(5)：$根据前面推导出来的公式，计算出尺度s，但是这里需要注意，针对的是单目相机，如果为深度或者双目相机，尺度默认为1：$$s=\frac{D}{S_P}=\frac{{\sum }_{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}\text{\hspace{1em}(06)}$$

$(6)：$根据公式求解出平移 $\mathbf{t}$ $$\mathbf{t}=\bar{Q}-s\mathbf{R}\bar{P}\text{\hspace{1em}(07)}$$
 
代码注释如下，位于 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(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));
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131

 

三、结语

通过一系列的讲解，可以说已经把 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);
1
2
3
4
5

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

 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件