ORB-SLAM（十）LoopClosing

构造函数

LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale);

主要分两部分，回环检测，以及GlobalBundleAdjustment

LoopClosing中的关键帧都是LocalMapping中送过来的：送过来一帧，就检查一帧。

// in LocalMapping::Run()
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
 
// in LoopClosing.cpp
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    if(pKF->mnId!=)
        mlpLoopKeyFrameQueue.push_back(pKF);
}

因此我们需要在KeyFrameDataBase中寻找与mlpLoopKeyFrameQueue相似的闭环候选帧。主要过程包括：

if(CheckNewKeyFrames())
{
    // Detect loop candidates and check covisibility consistency
    if(DetectLoop())
    {
    // Compute similarity transformation [sR|t]
    // In the stereo/RGBD case s=1
        if(ComputeSim3())
        {
            // Perform loop fusion and pose graph optimization
            CorrectLoop();
        }
    }
}

寻找回环候选帧，检查候选帧连续性；计算Sim3；闭环（地图点融合，位姿图优化）。

一. DetectLoop()

在共视关系中找到与当前关键帧Bow匹配最低得分minScore，在除去当前帧共视关系的关键帧数据库中，检测闭环候选帧：

vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

闭环候选帧筛选过程：

1. Bow得分>minScore；

2. 统计满足1的关键帧中有共同单词最多的单词数maxcommonwords；

3. 筛选出共同单词数大于mincommons(=0.8*maxcommons)的关键帧；

4. 相连的关键帧分为一组，计算组得分（总分），得到最大总分bestAccScore，筛选出总分大于minScoreToRetain(=0.75*bestAccScore)的组，用组中得分最高的候选帧lAccScoreAndMatch代表该组。计算组得分的目的是剔除单独一帧得分较高，但是没有共视关键帧，作为闭环来说不够鲁棒。

对于通过了闭环检测的关键帧，还需要通过连续性检测（连续三帧都通过上面的筛选），才能作为闭环候选帧。

mvpEnoughConsistentCandidates

二. ComputeSim3()

计算当前关键帧和闭环候选帧之间的Sim3，这个Sim3变换就是闭环前积累的尺度和位姿误差，该误差也可以帮助检验该闭环在空间几何姿态上是否成立。

1. Bow

通过SearchByBow搜索当前关键帧中和闭环候选帧匹配的地图点。(Bow通过将单词聚类到树结构node的方法，加快搜索匹配速度)

int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

若nmatches<20，剔除该候选帧，将匹配好的地图点放入当前候选帧对应的vvpMapPointMatches[i]中。

注意这里使用Bow匹配速度较快，但是会有漏匹配。

2. RANSAC

利用上面匹配上的地图点（虽然匹配上了，但是空间位置相差一个Sim3），用RANSAC方法求解Sim3位姿。（这里有可能求解不出Sim3，也就是虽然匹配满足，但是空间几何姿态不满足）。

这里Sim3的求解需要参考论文 Horn B K P. Closed-form solution of absolute orientation using unit quaternions. JOSA A, 1987, 4(4): 629-642.

Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
pSolver->SetRansacParameters(0.99,,);// 至少20个inliers 300次迭代

cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

3. SearchBySim3

根据计算出的Sim3（s，R，t），去找更多的匹配点（SearchBySim3），更新vpMapPointMatches

matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

4. Optimize

使用更新过的匹配，使用g2o优化Sim3位姿，这时内点数nInliers>20，才说明通过。一旦找到闭环帧mpMatchedKF，则break，跳过对其他候选闭环帧的判断。

const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, , mbFixScale);

5. SearchByProjection

获取mpMatchedKF及其相连关键帧对应的地图点。将这些地图点通过上面优化得到的Sim3（gScm-->mScw）变换投影到当前关键帧进行匹配，若匹配点>=40个，则返回ture，进行闭环调整，否则，返回false，线程暂停5ms后继续检查Tracking发送来的关键帧队列。

matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,);

注意这里得到的当前关键帧中匹配上闭环关键帧共视地图点（mvpCurrentMatchedPoints），将用于后面CorrectLoop时当前关键帧地图点的冲突融合。

到这里，不仅确保了当前关键帧与闭环帧之间匹配度高，而且保证了闭环帧的共视图中的地图点和当前关键帧的特征点匹配度更高（20--->40），说明该闭环帧是正确的。

三. CorrectLoop()

闭环纠正时，LocalMapper和Global BA必须停止。注意Global BA使用的是单独的线程。

分为两步，第一步LoopFusion，第二步Essential Graph优化。

其中Essential Graph包含三部分：1. 共视关系很好的关键帧；2. spanning tree连接关系（父子关系）；3. 闭环关键帧连接关系。

使用计算出的Sim3对当前位姿进行调整，并且传播到当前关键帧相连的关键帧（相连关键帧之间相对位姿是知道的，通过当前关键帧的Sim3计算相连关键帧的Sim3）。这样回环的两侧关键帧就对齐了，利用调整过的位姿更新这些相连关键帧对应的地图点。然后将闭环帧及其相连帧的地图点都投影到当前帧以及相连帧上，投影匹配上的和Sim3计算过的地图点进行融合（就是替换成质量高的），涉及融合的关键帧还需要更新其在共视地图中的观测边关系，这是为了剥离出因为闭环产生的新的连接关系LoopConnections，用于优化Essential Graph。添加当前帧与闭环匹配帧之间的边，该边不参与优化。

记住地图点是连接关键帧之间的枢纽，每次调整地图点位置后都需要更新关键帧的连接关系。

下面新开一个线程进行全局优化。OptimizeEssentialGraph只是优化一些主要关键帧，全局优化可以优化所有的位姿与MapPoints。