(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
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 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=1nPiQiT= SxxSyxSzxSxySyySzySxzSyzSzz (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=SPD=i=1nPi2i=1nQiRPi                           s=SPSQ =i=1nPi2i=1nQi2 (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<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 文件。

( 1 ) : \color{blue}(1): (1) 首先计算3D点质心及去质心后的点,也就是对应前面博客推导的如下公式:
P ˉ = 1 n ∑ i = 1 n P i                       Q ˉ = 1 n ∑ i = 1 n Q i (01) \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} Pˉ=n1i=1nPi                     Qˉ=n1i=1nQi(01) P i ′ = P i − P ˉ                        Q i ′ = Q i − Q ˉ (02) \color{Green} \tag{02} P_{i}^{\prime}=P_{i}-\bar{P}~~~~~~~~~~~~~~~~~~~~~~Q_{i}^{\prime}=Q_{i}-\bar{Q} Pi=PiPˉ                      Qi=QiQˉ(02)

( 2 ) : \color{blue}(2): (2) 计算 M M M N N N:
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 ] (03) \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} M=i=1nPiQiT= SxxSyxSzxSxySyySzySxzSyzSzz (03) N = ∑ i = 1 n R Q , i T R P , i ‾ = [ ( S x x + S y y + S z z ) S y z − S z y S z x − S x z S x y − S y x S y z − S z y ( S x x − S y y − S z z ) S x y + S y x S z x + S x z S z x − S x z S x y + S y x ( − S x x + S y y − S z z ) S y z + S z y S x y − S y x S z x + S x z S y z + S z y ( − S x x − S y y + S z z ) ] (04) \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} N=i=1nRQ,iTRP,i= (Sxx+Syy+Szz)SyzSzySzxSxzSxySyxSyzSzy(SxxSyySzz)Sxy+SyxSzx+SxzSzxSxzSxy+Syx(Sxx+SyySzz)Syz+SzySxySyxSzx+SxzSyz+Szy(SxxSyy+Szz) (04)
 
( 3 ) : \color{blue}(3): (3) 调用 cv::eigen(N,eval,evec) 函数计算对称矩阵特征值和特征向量,特征值默认是从大到小排列,所以evec[0] 是最大值。
 
( 4 ) : \color{blue}(4): (4)先四元数转换成旋转向量 vec,然后再调用cv::Rodrigues(vec,mR12i)函数,即可以把旋转向量转换成旋转矩阵。
θ = 2 arccos ⁡ w                      vec = [ n x , n y , n z ] T = [ x , y , z ] T / sin ⁡ θ 2 (05) \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} θ=2arccosw                     vec=[nx,ny,nz]T=[x,y,z]T/sin2θ(05)

( 5 ) : \color{blue}(5): (5)根据前面推导出来的公式,计算出尺度s,但是这里需要注意,针对的是单目相机,如果为深度或者双目相机,尺度默认为1: s = D S P = ∑ i = 1 n Q i ′ R P i ′ ∑ i = 1 n ∥ P i ′ ∥ 2 (06) \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}} s=SPD=i=1nPi2i=1nQiRPi(06)

( 6 ) : \color{blue}(6): (6)根据公式求解出平移 t \mathbf t t t = Q ˉ − s R P ˉ (07) \color{Green} \tag{07} \mathbf t=\bar{Q}-s \mathbf R \bar{P} t=QˉsRPˉ(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));
}

 

三、结语

通过一系列的讲解,可以说已经把 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() 函数涉及到图优化,再后面有专门的章节进行统一的讲解。

 
 
 

你可能感兴趣的:(#,概率论,人工智能,ORB-SLAM2,机器人,无人机)