ORB-SLAM2 LoopClosing.cc 读代码

这块是看其他地方的时候看到了，所以提前来先写点，这篇写不完会拖几天，先能写多少写多少

一、计算当前关键帧和上一步闭环候选关键帧Sim3变换 

/**
 * @brief 计算当前关键帧和上一步闭环候选帧的Sim3变换
 * 1. 遍历闭环候选帧集，筛选出与当前帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
 * 2. 对每一个候选帧进行 Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
 * 3. 取出闭环匹配上关键帧的相连关键帧，得到它们的地图点放入 mvpLoopMapPoints
 * 4. 将闭环匹配上关键帧以及相连关键帧的地图点投影到当前关键帧进行投影匹配
 * 5. 判断当前帧与检测出的所有闭环关键帧是否有足够多的地图点匹配
 * 6. 清空mvpEnoughConsistentCandidates
 * @return true         只要有一个候选关键帧通过Sim3的求解与优化，就返回true
 * @return false        所有候选关键帧与当前关键帧都没有有效Sim3变换
 */
bool LoopClosing::ComputeSim3()
{
    // Sim3 计算流程说明：
    // 1. 通过Bow加速描述子的匹配，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3（当前帧---闭环帧）          
    // 2. 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3（当前帧---闭环帧）   
    // 3. 将闭环帧以及闭环帧相连的关键帧的地图点与当前帧的点进行匹配（当前帧---闭环帧+相连关键帧）     
    // 注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中，实际的更新步骤见CorrectLoop()步骤3
    // 对于双目或者是RGBD输入的情况,计算得到的尺度=1


    //  准备工作
    // For each consistent loop candidate we try to compute a Sim3
    // 对每个（上一步得到的具有足够连续关系的）闭环候选帧都准备算一个Sim3
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();

    // We compute first ORB matches for each candidate
    // If enough matches are found, we setup a Sim3Solver
    ORBmatcher matcher(0.75,true);

    // 存储每一个候选帧的Sim3Solver求解器
    vector<Sim3Solver*> vpSim3Solvers;
    vpSim3Solvers.resize(nInitialCandidates);

    // 存储每个候选帧的匹配地图点信息
    vector<vector<MapPoint*> > vvpMapPointMatches;
    vvpMapPointMatches.resize(nInitialCandidates);

    // 存储每个候选帧应该被放弃(True）或者 保留(False)
    vector<bool> vbDiscarded;
    vbDiscarded.resize(nInitialCandidates);

    // 完成 Step 1 的匹配后，被保留的候选帧数量
    int nCandidates=0;

    // Step 1. 遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
    for(int i=0; i<nInitialCandidates; i++)
    {
        // Step 1.1 从筛选的闭环候选帧中取出一帧有效关键帧pKF
        KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

        // 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
        pKF->SetNotErase();

        // 如果候选帧质量不高，直接PASS
        if(pKF->isBad())
        {
            vbDiscarded[i] = true;
            continue;
        }

        // Step 1.2 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
        // 通过bow加速得到 mpCurrentKF 与 pKF 之间的匹配特征点
        // vvpMapPointMatches 是匹配特征点对应的地图点,本质上来自于候选闭环帧
        int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

        // 粗筛：匹配的特征点数太少，该候选帧剔除
        if(nmatches<20)
        {
            vbDiscarded[i] = true;
            continue;
        }
        else
        {
            // Step 1.3 为保留的候选帧构造Sim3求解器
            // 如果 mbFixScale（是否固定尺度） 为 true，则是6 自由度优化（双目 RGBD）
            // 如果是false，则是7 自由度优化（单目）
            Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);

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

        // 保留的候选帧数量
        nCandidates++;
    }

    // 用于标记是否有一个候选帧通过Sim3Solver的求解与优化
    bool bMatch = false;

    // Step 2 对每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
    while(nCandidates>0 && !bMatch)
    {
        // 遍历每一个候选帧
        for(int i=0; i<nInitialCandidates; i++)
        {
            if(vbDiscarded[i])
                continue;

            KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

            // 内点（Inliers）标志
            // 即标记经过RANSAC sim3 求解后,vvpMapPointMatches中的哪些作为内点
            vector<bool> vbInliers; 
        
            // 内点（Inliers）数量
            int nInliers;

            // 是否到达了最优解
            bool bNoMore;

            // Step 2.1 取出从 Step 1.3 中为当前候选帧构建的 Sim3Solver 并开始迭代
            Sim3Solver* pSolver = vpSim3Solvers[i];

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

            // If Ransac reachs max. iterations discard keyframe
            // 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

            // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
            // 如果计算出了Sim3变换，继续匹配出更多点并优化。因为之前 SearchByBoW 匹配可能会有遗漏
            if(!Scm.empty())
            {
                // 取出经过Sim3Solver 后匹配点中的内点集合
                vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
                for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
                {
                    // 保存内点
                    if(vbInliers[j])
                       vpMapPointMatches[j]=vvpMapPointMatches[i][j];
                }

                // Step 2.2 通过上面求取的Sim3变换引导关键帧匹配，弥补Step 1中的漏匹配
                // 候选帧pKF到当前帧mpCurrentKF的R（R12），t（t12），变换尺度s（s12）
                cv::Mat R = pSolver->GetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();

                // 查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数，之前使用SearchByBoW进行特征点匹配时会有漏匹配）
                // 通过Sim3变换，投影搜索pKF1的特征点在pKF2中的匹配，同理，投影搜索pKF2的特征点在pKF1中的匹配
                // 只有互相都成功匹配的才认为是可靠的匹配
                matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

                // Step 2.3 用新的匹配来优化 Sim3，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
                // OpenCV的Mat矩阵转成Eigen的Matrix类型
                // gScm：候选关键帧到当前帧的Sim3变换
                g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
            
                // 如果mbFixScale为true，则是6 自由度优化（双目 RGBD），如果是false，则是7 自由度优化（单目）
                // 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

                // 如果优化成功，则停止while循环遍历闭环候选
                if(nInliers>=20)
                {
                    // 为True时将不再进入 while循环
                    bMatch = true;
                    // mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧
                    mpMatchedKF = pKF;

                    // gSmw：从世界坐标系 w 到该候选帧 m 的Sim3变换，都在一个坐标系下，所以尺度 Scale=1
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);

                    // 得到g2o优化后从世界坐标系到当前帧的Sim3变换
                    mg2oScw = gScm*gSmw;
                    mScw = Converter::toCvMat(mg2oScw);
                    mvpCurrentMatchedPoints = vpMapPointMatches;

                    // 只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
                    break;
                }
            }
        }
    }

    // 退出上面while循环的原因有两种,一种是求解到了bMatch置位后出的,另外一种是nCandidates耗尽为0
    if(!bMatch)
    {
        // 如果没有一个闭环匹配候选帧通过Sim3的求解与优化
        // 清空mvpEnoughConsistentCandidates，这些候选关键帧以后都不会在再参加回环检测过程了
        for(int i=0; i<nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        // 当前关键帧也将不会再参加回环检测了
        mpCurrentKF->SetErase();
        // Sim3 计算失败，退出了
        return false;
    }

    // Step 3：取出与当前帧闭环匹配上的关键帧及其共视关键帧，以及这些共视关键帧的地图点
    // 注意是闭环检测出来与当前帧形成闭环的关键帧 mpMatchedKF
    // 将mpMatchedKF共视的关键帧全部取出来放入 vpLoopConnectedKFs
    // 将vpLoopConnectedKFs的地图点取出来放入mvpLoopMapPoints
    vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();

    // 包含闭环匹配关键帧本身,形成一个“闭环关键帧小组“
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    mvpLoopMapPoints.clear();

    // 遍历这个组中的每一个关键帧
    for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
    {
        KeyFrame* pKF = *vit;
        vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();

        // 遍历其中一个关键帧的所有有效地图点
        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                // mnLoopPointForKF 用于标记，避免重复添加
                if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
                {
                    mvpLoopMapPoints.push_back(pMP);
                    // 标记一下
                    pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                }
            }
        }
    }

    // Find more matches projecting with the computed Sim3
    // Step 4：将闭环关键帧及其连接关键帧的所有地图点投影到当前关键帧进行投影匹配
    // 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
    // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配对
    // mvpCurrentMatchedPoints是前面经过SearchBySim3得到的已经匹配的点对，这里就忽略不再匹配了
    // 搜索范围系数为10
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

    // If enough matches accept Loop
    // Step 5: 统计当前帧与闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败
    int nTotalMatches = 0;
    for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
    {
        if(mvpCurrentMatchedPoints[i])
            nTotalMatches++;
    }

    if(nTotalMatches>=40)
    {
        // 如果当前回环可靠,保留当前待闭环关键帧，其他闭环候选全部删掉以后不用了
        for(int i=0; i<nInitialCandidates; i++)
            if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                mvpEnoughConsistentCandidates[i]->SetErase();
        return true;
    }
    else   
    {
        // 闭环不可靠，闭环候选及当前待闭环帧全部删除
        for(int i=0; i<nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }
}

先声明几个成员的含义：

1. mvpEnoughConsistentCandidates  好的闭环候选帧的向量

2. pKF 具体某一个闭环候选帧

3. mpCurrentKF 当前关键帧

4. vpMapPointMatches    经过Sim3Slover后匹配点中内点的合集

5. mvpCurrentMatchedPoints  地图点在当前关键帧中找到的匹配地图点的合集

6.mpMatchedKF 最终检测出来的和当前关键帧闭环的闭环关键帧

7.mvpLoopMapPoints 闭环关键帧上所有相连关键帧的地图点

该代码的流程为：

1.获取好的闭环候选帧的个数，存到 nInitialCandidates 中

2. 构造ORBmatcher ，最优评分和次优评分比例为0.75

3. 初始化Sim3Slover优化器队列  vpSim3Solvers ，大小与 nInitialCandidates 相同

4. 初始化二维向量，类型为地图点类型 ，vvpMapPointMatches ，大小与nInitialCandidates 相同，用来存储每个候选帧的地图点

 5. 初始化bool类型向量 vbDiscarded，大小与 nInitialCandidates 相同 ，用来表示各个候选帧是被保留（False）还是丢弃(True)

6.初始化保留的候选帧的个数  nCandidates 为 0

7. 遍历每个好的闭环候选帧

        1）取出其中关键帧，将该关键帧设置为不被剔除

        2）如果该关键帧的质量不高，则退出本次循环，否则接着往下跑

        3）通过词袋计算当前关键帧（这里计为F1）和 正在遍历的关键帧（这里计为F2），将F2中与F1匹配上的地图点存到 vvpMapPointMatches 中， vvpMapPointMatches 第一维为F1中第几个地图点（索引），第二维为F2中匹配点的索引。 nmatches 记录两帧中匹配到的地图点的数目。

        4）如果两帧匹配的地图点太少，则退出本次循环，否则接着往下跑

        5）构造Sim3Solver ,输入为当前关键帧（上面提到的F1）、正在遍历的关键帧（这里计为F2）,匹配点对 vvpMapPointMatches ，要求slover的置信度为0.99,至少20个内点，最多300次匹配。

        6）   nCandidates自增   

8. 将记录是否有候选帧求解成功的标志  bMatch 写false

9.  遍历所有候选帧，直到所有候选帧都遍历或找到求解成功的候选帧 (这里有个问题算到一个求解成功的就结束这样会出错吗？而且下面代码的写法就已经导致了也就只能拿一个求解成功的候选帧了)

        1） 如果该候选帧被丢弃，则退出本次循环

        2）  取出当前候选帧的 Sim3Solver,做正在遍历的关键帧（F2）到当前帧的Sim3（F1）变换（T12），最多迭代5次。

        3）如果迭代后要求不达标，将当前帧丢弃vbDiscarded 对应索引写true,将候选帧个数 nCandidates自减。

        4） 如果Sim3计算出结果，遍历所有Sim3得到的内点， 取出Sim3计算得到的点对， 将其写到     vpMapPointMatches ，获取Sim3出来的旋转矩阵、平移矩阵和变换尺度。

        5）通过ORBmatcher获取当前输入帧的特征点在该候选帧中的匹配，存放到 vpMapPointMatches 中。这块后面单独讲。

        6） 通过G2O去优化候选关键帧到当前帧的Sim3变换，并获取内点数，如果内点大于20个则停止整个遍历循环，将找到匹配帧标志设置为true, 将当前候选帧赋值给mpMatchedKF

10.如果遍历完所有候选帧，没有匹配帧（第一个情况，没有候选点，第二个情况是求解不到匹配帧），就释放所有 mvpEnoughConsistentCandidates ， mpCurrentKF 当前关键帧释放不参加回环检测。

11. 取出与 最终检测出来的和当前关键帧闭环的闭环关键帧 mpMatchedKF 连接的关键帧 （这里其实是共视帧）

12 . 将11步取出的共视关键帧和上面获得 的mpMatchedKF 放入容器中，遍历这些关键帧

        1）取出各个关键帧的地图点，遍历各个地图点，如果不是坏点且该点没有标记过，把这个地图点压到 mvpLoopMapPoints 中，且标记的关键帧计为当前帧

13. 通过ORBmatcher根据Sim3变换，当闭环关键帧及其共视帧的所有地图点，投影到当前关键帧，生成新的匹配点对，方法为 SearchByProjection ，这里到时候单独讲。

14.如果第13步输出匹配点对数大于等于40个，遍历   好的闭环候选帧的向量 mvpEnoughConsistentCandidates ，如果与  最终检测出来的和当前关键帧闭环的闭环关键帧mpMatchedKF 不同，则 删除该关键帧。  如果输出的匹配点对小于40个，释放mvpEnoughConsistentCandidates 以及当前关键帧  mpCurrentKF