ORBSLAM2源码小记(7)--回环检测器LoopClosing

ORBSLAM2的回环检测器LoopClosing

主要是记录下相对容易忘的内容，不会写的很细

回环检测器LoopClosing

ORBSlam2会对每个关键帧检测闭环，整体流程其实就三步：检测闭环、计算变换、闭环矫正。这里面主要的问题是如何检测闭环，必须要严格限制闭环检测的灵敏度，因为错误的闭环的代价非常高。

在检测闭环时，会首先分组并通过计算得分的方式来筛选出闭环候选帧，然后需要保证至少连续三个关键帧都能和闭环候选帧的组有匹配，才认为满足了闭环（后面会细说，大致就是要求一个时间窗口内的关键帧能和已有地图中的一个时间窗口内的关键帧匹配上）

回环检测

在回环检测时，首先使用当前帧的Bow用于匹配器来匹配出闭环候选帧（如何匹配的已经在前文描述了，和重定位类似，都有一个组的概念）。获取到闭环候选帧后，接下来的操作才是重点，为了避免错误闭环，ORBSlam2假设当前帧为A帧，若A帧存在闭环候选帧B帧时，A帧未来几个关键帧也能闭环上且与B帧接下来几个关键帧是有部分视野重合。

那如何体现这个假设呢？ORBSlam2是通过比较共视关键帧是否有重合来描述的。具体而言，记A帧后面若干关键帧分别为A1，A2...，其候选关键帧存在且对应的闭环候选关键帧为B1，B2...，既然Bi已经是Ai的闭环候选关键帧了，那么说明他们之间一定非常相似，不然肯定无法匹配上因此只需要去判断B帧，B1帧，B2帧...是否存在重合视野即可。

这时，将B帧和其共视关键帧一起作为B帧组，同理也得到B1帧组，B2帧组...然后从B帧组开始，判断B帧组与B1帧组是否存在某个相同的关键帧(即某个关键帧即为B帧的共视，又为B1帧的共视)，如果存在这样一个关键帧，就认为B帧组和B1帧组连续。当存在三个组连续时，那么才认为真的构成闭环了。（注意这个过程中的A1，A2...一定要存在闭环候选帧，如果不存在闭环候选帧那么就会清空所有组然后重新计数）

这里存在三个组"连续"并不要求是三个连续的组"连续"

比如当A1，A2...A10均存在闭环候选关键帧B1...B10时，连续的三个组可以是：

• B1组，B2组，B3组
• B1组，B3组，B8组
• B6组，B8组，B9组

因为统计时是使用元组来计数的，假设某次计数后的元组为(B1,0), (B2,0), (B3,1)，新来的组为B4组，且和B3组"连续"，那么接下来元组就会变为(B1,0), (B2,0), (B3,1), (B4,2)

下面这个表做了一个模拟

时刻	候选帧	有共视	mvConGroup(before)	mvConGroup(after)
0	C0	无候选	任意	空
1	C1	-	空	(C1,0)
2	C2	-	(C1,0)	(C1,0), (C2,0)
3	C3	C1组	(C1,0), (C2,0)	(C1,0), (C2,0),(C3,1)
4	C4	C2组	(C1,0), (C2,0),(C3,1)	(C1,0), (C2,0),(C3,1), (C4,1)
5	C5	C4组	(C1,0), (C2,0),(C3,1), (C4,1)	(C1,0), (C2,0),(C3,1), (C4,1), (C5,2)
6	C6	C5组	(C1,0), (C2,0),(C3,1), (C4,1), (C5,2)	(C1,0), (C2,0),(C3,1), (C4,1), (C5,2), (C6,3)

/**
 * @brief 闭环检测
 *
 * @return true             成功检测到闭环
 * @return false            未检测到闭环
 */
bool LoopClosing::DetectLoop()
{
    ...

    // 在所有关键帧中找出闭环候选帧（注意不和当前帧连接）
    vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

    ...

    // 遍历刚才得到的每一个候选关键帧
    for (size_t i = 0, iend = vpCandidateKFs.size(); i < iend; i++)
    {
        KeyFrame *pCandidateKF = vpCandidateKFs[i];

        // 将自己以及与自己相连的关键帧构成一个“子候选组”
        set<KeyFrame *> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        spCandidateGroup.insert(pCandidateKF);

        // 连续性达标的标志
        bool bEnoughConsistent = false;
        bool bConsistentForSomeGroup = false;

        // 遍历前一次闭环检测到的连续组链
        for (size_t iG = 0, iendG = mvConsistentGroups.size(); iG < iendG; iG++)
        {
            // 取出之前的一个子连续组中的关键帧集合
            set<KeyFrame *> sPreviousGroup = mvConsistentGroups[iG].first;

            // 遍历每个“子候选组”，检测子候选组中每一个关键帧在“子连续组”中是否存在
            // 如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续
            bool bConsistent = false;
            for (set<KeyFrame *>::iterator sit = spCandidateGroup.begin(), send = spCandidateGroup.end(); sit != send; sit++)
            {
                if (sPreviousGroup.count(*sit))
                {
                    // 如果存在，该“子候选组”与该“子连续组”相连
                    bConsistent = true;
                    // 该“子候选组”至少与一个”子连续组“相连，跳出循环
                    bConsistentForSomeGroup = true;
                    break;
                }
            }

            if (bConsistent)
            {
                // 如果判定为连续，接下来判断是否达到连续的条件
                // 取出和当前的候选组发生"连续"关系的子连续组的"已连续次数"
                int nPreviousConsistency = mvConsistentGroups[iG].second;
                // 将当前候选组连续长度在原子连续组的基础上 +1，
                int nCurrentConsistency = nPreviousConsistency + 1;
                // 如果上述连续关系还未记录到 vCurrentConsistentGroups，那么记录一下
                // 注意这里spCandidateGroup 可能放置在vbConsistentGroup中其他索引(iG)下
                if (!vbConsistentGroup[iG])
                {
                    // 将该“子候选组”的该关键帧打上连续编号加入到“当前连续组”
                    ConsistentGroup cg = make_pair(spCandidateGroup, nCurrentConsistency);
                    // 放入本次闭环检测的连续组vCurrentConsistentGroups里
                    vCurrentConsistentGroups.push_back(cg);
                    // this avoid to include the same group more than once
                    //  标记一下，防止重复添加到同一个索引iG
                    //  但是spCandidateGroup可能重复添加到不同的索引iG对应的vbConsistentGroup 中
                    vbConsistentGroup[iG] = true;
                }
                // 如果连续长度满足要求，那么当前的这个候选关键帧是足够靠谱的
                if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent)
                {
                    // 记录为达到连续条件了
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    ...
                }
            }
        }

        // 如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系
        // 于是就把“子候选组”全部拷贝到 vCurrentConsistentGroups， 用于更新mvConsistentGroups，连续性计数器设为0
        if (!bConsistentForSomeGroup)
        {
            ConsistentGroup cg = make_pair(spCandidateGroup, 0);
            vCurrentConsistentGroups.push_back(cg);
        }
    } 

    // 返回闭环检测结果
    ...
}

Sim3计算

检测到闭环后需要计算Sim3变换，其实就是两个闭环帧的相对位姿+尺度，需要尺度是因为单目会存在尺度漂移，需要闭环来纠正这个尺度漂移。

ORBSLAM2按以下步骤求解Sim3：

• 首先用Bow匹配两个闭环帧的匹配点
• 然后基于上述匹配点来构造Sim3求解器结合RANSAC求解Sim3位姿（具体怎么求解的就不展开了，也是比较基础的东西）
• 使用Sim3扩大匹配，并将上述求出来了Sim3位姿作为初始值，用优化的方法精细化求解
• 将闭环候选帧及其共视帧的地图点投影到当前关键帧上，以扩大匹配
• 判断扩大匹配后的匹配点数目，如果有足够的匹配点数目才认为真的算出了Sim3

/**
 * @brief 计算当前关键帧和上一步闭环候选帧的Sim3变换
 * 1. 遍历闭环候选帧集，筛选出与当前帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
 * 2. 对每一个候选帧进行 Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
 * 3. 取出闭环匹配上关键帧的相连关键帧，得到它们的地图点放入 mvpLoopMapPoints
 * 4. 将闭环匹配上关键帧以及相连关键帧的地图点投影到当前关键帧进行投影匹配
 * 5. 判断当前帧与检测出的所有闭环关键帧是否有足够多的地图点匹配
 * 6. 清空mvpEnoughConsistentCandidates
 * @return true         只要有一个候选关键帧通过Sim3的求解与优化，就返回true
 * @return false        所有候选关键帧与当前关键帧都没有有效Sim3变换
 */
bool LoopClosing::ComputeSim3()
{
    {
        ...
        // 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
        int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]);

        // 粗筛：匹配的特征点数太少，该候选帧剔除
        if (nmatches < 20)
        {
            vbDiscarded[i] = true;
            continue;
        }
        else
        {
            // 为保留的候选帧构造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++;
    }


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

            // 为当前候选帧构建 Sim3Solver 并开始迭代
            Sim3Solver *pSolver = vpSim3Solvers[i];
            // 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
            cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
            // 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
            if (bNoMore)
            {
                vbDiscarded[i] = true;
                nCandidates--;
            }
            // 如果计算出了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];
                }

                ...

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

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

    ...

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

    // 统计当前帧与闭环关键帧的匹配地图点数目，超过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;
    }
}

回环矫正

计算好了Sim3就可以正式开始闭环融合了，主要有以下步骤：

1. 更新由于Sim3计算时扩大匹配引起的共视关系变化
2. 固定当前帧的Sim3，当前帧共视帧相对闭环候选帧的Sim3
3. 基于此Sim3来更新共视帧和当前帧的地图点，包括观测方向等信息
4. 基于此Sim3来更新共视帧和当前帧的位姿，包括共视关系
5. 将闭环关键帧组地图点投影到当前关键帧组中，融合地图点，优先使用闭环时新增的地图点替换之前的地图点
6. 本质图优化，更新Sim3和地图点
7. 新建全局BA线程来优化所有关键帧的位姿和地图点的位姿

全局BA就是优化重投影误差，所有关键帧和地图点都会参与优化，没什么好说的。值得注意的是这里的本质图优化，它优化的是Sim3位姿，前文在计算Sim3时也有一个优化是优化Sim3位姿的，他们的共性是均不优化地图点，但存在一些区别：

• 本质图优化：优化多个关键帧，用于调整这些关键帧之间相对的Sim3，这些关键帧包括
  • 闭环时因为地图点调整而出现的关键帧间的新连接关系
  • 生成树的父子关键帧
  • 当前帧与闭环匹配帧之间的连接关系(这里面也包括了当前遍历到的这个关键帧之前曾经存在过的回环边)
  • 共视程度超过100的关键帧
• 计算Sim3时的优化：只包括当前关键帧和闭环候选关键帧两个关键帧，用于精细求解两帧之间的Sim3

/**
 * @brief 闭环矫正
 * 1. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的地图点位置（相连关键帧---当前帧）
 * 2. 将闭环帧以及闭环帧相连的关键帧的地图点和与当前帧相连的关键帧的点进行匹配（当前帧+相连关键帧---闭环帧+相连关键帧）
 * 3. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph
 * 4. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整
 * 5. 创建线程进行全局Bundle Adjustment
 */
void LoopClosing::CorrectLoop()
{
    // Step 1：根据共视关系更新当前帧与其它关键帧之间的连接
    // Step 2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
    // Step 3：检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
    // Step 4：通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
    // Step 5：更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
    // Step 6：进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系
    // Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
    // Step 8：新建一个线程用于全局BA优化

    ...

    // 根据共视关系更新当前关键帧与其它关键帧之间的连接关系
    // 因为之前闭环检测、计算Sim3中改变了该关键帧的地图点，所以需要更新
    mpCurrentKF->UpdateConnections();

    // 通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点
    // 当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，
    // 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换

    // 取出当前关键帧及其共视关键帧，称为“当前关键帧组”
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);
    // CorrectedSim3：存放闭环g2o优化后当前关键帧的共视关键帧的世界坐标系下Sim3 变换
    // NonCorrectedSim3：存放没有矫正的当前关键帧的共视关键帧的世界坐标系下Sim3 变换
    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
    // 先将mpCurrentKF的Sim3变换存入，认为是准的，所以固定不动
    CorrectedSim3[mpCurrentKF] = mg2oScw;
    // 当前关键帧到世界坐标系下的变换矩阵
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();

    // 对地图点操作
    {
        // Get Map Mutex
        // 锁定地图点
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

        // Step 2.1：通过mg2oScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿
        // 遍历"当前关键帧组""
        for (vector<KeyFrame *>::iterator vit = mvpCurrentConnectedKFs.begin(), vend = mvpCurrentConnectedKFs.end(); vit != vend; vit++)
        {
            KeyFrame *pKFi = *vit;
            cv::Mat Tiw = pKFi->GetPose();
            if (pKFi != mpCurrentKF) // 跳过当前关键帧，因为当前关键帧的位姿已经在前面优化过了，在这里是参考基准
            {
                // 得到当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的相对变换
                cv::Mat Tic = Tiw * Twc;
                cv::Mat Ric = Tic.rowRange(0, 3).colRange(0, 3);
                cv::Mat tic = Tic.rowRange(0, 3).col(3);

                // g2oSic：当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的Sim3 相对变换
                // 这里是non-correct, 所以scale=1.0
                g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric), Converter::toVector3d(tic), 1.0);
                // 当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿
                g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw;
                // 存放闭环g2o优化后当前关键帧的共视关键帧的Sim3 位姿
                CorrectedSim3[pKFi] = g2oCorrectedSiw;
            }

            cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);
            cv::Mat tiw = Tiw.rowRange(0, 3).col(3);
            g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);
            // 存放没有矫正的当前关键帧的共视关键帧的Sim3变换
            NonCorrectedSim3[pKFi] = g2oSiw;
        }

        // 得到矫正的当前关键帧的共视关键帧位姿后，修正这些共视关键帧的地图点
        // 遍历待矫正的共视关键帧（不包括当前关键帧）
        for (KeyFrameAndPose::iterator mit = CorrectedSim3.begin(), mend = CorrectedSim3.end(); mit != mend; mit++)
        {
            // 取出当前关键帧连接关键帧
            KeyFrame *pKFi = mit->first;
            // 取出经过位姿传播后的Sim3变换
            g2o::Sim3 g2oCorrectedSiw = mit->second;
            g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
            // 取出未经过位姿传播的Sim3变换
            g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];

            vector<MapPoint *> vpMPsi = pKFi->GetMapPointMatches();
            // 遍历待矫正共视关键帧中的每一个地图点
            for (size_t iMP = 0, endMPi = vpMPsi.size(); iMP < endMPi; iMP++)
            {
                MapPoint *pMPi = vpMPsi[iMP];
                // 跳过无效的地图点
                if (!pMPi)
                    continue;
                if (pMPi->isBad())
                    continue;
                // 标记，防止重复矫正
                if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId)
                    continue;

                // 矫正过程本质上也是基于当前关键帧的优化后的位姿展开的
                // 将该未校正的eigP3Dw先从世界坐标系映射到未校正的pKFi相机坐标系，然后再反映射到校正后的世界坐标系下
                cv::Mat P3Dw = pMPi->GetWorldPos();
                // 地图点世界坐标系下坐标
                Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);
                // map(P) 内部做了相似变换 s*R*P +t
                // 下面变换是：eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ world
                Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));

                cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                pMPi->SetWorldPos(cvCorrectedP3Dw);
                // 记录矫正该地图点的关键帧id，防止重复
                pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
                // 记录该地图点所在的关键帧id
                pMPi->mnCorrectedReference = pKFi->mnId;
                // 因为地图点更新了，需要更新其平均观测方向以及观测距离范围
                pMPi->UpdateNormalAndDepth();
            }

            // 将共视关键帧的Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
            // 其实是现在已经有了更新后的关键帧组中关键帧的位姿,但是在上面的操作时只是暂时存储到了 KeyFrameAndPose 类型的变量中,还没有写回到关键帧对象中
            // 调用toRotationMatrix 可以自动归一化旋转矩阵
            Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
            Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
            double s = g2oCorrectedSiw.scale();
            // 平移向量中包含有尺度信息，还需要用尺度归一化
            eigt *= (1. / s);

            cv::Mat correctedTiw = Converter::toCvSE3(eigR, eigt);
            // 设置矫正后的新的pose
            pKFi->SetPose(correctedTiw);

            // 根据共视关系更新当前帧与其它关键帧之间的连接
            // 地图点的位置改变了,可能会引起共视关系\权值的改变
            pKFi->UpdateConnections();
        }

        // 检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补
        // mvpCurrentMatchedPoints 是当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点
        for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++)
        {
            if (mvpCurrentMatchedPoints[i])
            {
                // 取出同一个索引对应的两种地图点，决定是否要替换
                //  匹配投影得到的地图点
                MapPoint *pLoopMP = mvpCurrentMatchedPoints[i];
                // 原来的地图点
                MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i);
                if (pCurMP)
                    // 如果有重复的MapPoint，则用匹配的地图点代替现有的
                    // 因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差
                    pCurMP->Replace(pLoopMP);
                else
                {
                    // 如果当前帧没有该MapPoint，则直接添加
                    mpCurrentKF->AddMapPoint(pLoopMP, i);
                    pLoopMP->AddObservation(mpCurrentKF, i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        }
    }

    // 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，融合，新增或替换当前关键帧组中KF的地图点
    // 因为闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的
    // 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
    // CorrectedSim3：存放矫正后当前关键帧的共视关键帧，及其世界坐标系下Sim3 变换
    SearchAndFuse(CorrectedSim3);

    // 更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系
    // LoopConnections：存储因为闭环地图点调整而新生成的连接关系
    map<KeyFrame *, set<KeyFrame *>> LoopConnections;

    // 遍历当前帧相连关键帧组（一级相连）
    for (vector<KeyFrame *>::iterator vit = mvpCurrentConnectedKFs.begin(), vend = mvpCurrentConnectedKFs.end(); vit != vend; vit++)
    {
        KeyFrame *pKFi = *vit;
        // 得到与当前帧相连关键帧的相连关键帧（二级相连）
        vector<KeyFrame *> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();

        // 更新一级相连关键帧的连接关系(会把当前关键帧添加进去,因为地图点已经更新和替换了)
        pKFi->UpdateConnections();
        // 取出该帧更新后的连接关系
        LoopConnections[pKFi] = pKFi->GetConnectedKeyFrames();
        // 从连接关系中去除闭环之前的二级连接关系，剩下的连接就是由闭环得到的连接关系
        for (vector<KeyFrame *>::iterator vit_prev = vpPreviousNeighbors.begin(), vend_prev = vpPreviousNeighbors.end(); vit_prev != vend_prev; vit_prev++)
        {
            LoopConnections[pKFi].erase(*vit_prev);
        }
        // 从连接关系中去除闭环之前的一级连接关系，剩下的连接就是由闭环得到的连接关系
        for (vector<KeyFrame *>::iterator vit2 = mvpCurrentConnectedKFs.begin(), vend2 = mvpCurrentConnectedKFs.end(); vit2 != vend2; vit2++)
        {
            LoopConnections[pKFi].erase(*vit2);
        }
    }

    // 进行本质图优化，优化本质图中所有关键帧的位姿和地图点
    // LoopConnections是形成闭环后新生成的连接关系，不包括当前帧与闭环匹配帧之间的连接关系
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

    ...

    // 新建一个线程用于全局BA优化
    // OptimizeEssentialGraph只是优化了一些主要关键帧的位姿，这里进行全局BA可以全局优化所有位姿和MapPoints
    mbRunningGBA = true;
    mbFinishedGBA = false;
    mbStopGBA = false;
    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId);

    ...
}