ORB-SLAM2 --- LoopClosing::CorrectLoop函数

目录

1.函数作用

2.函数流程 

3.code

4.函数解析

4.1 结束局部地图线程、全局BA，为闭环矫正做准备

4.2 根据共视关系更新当前关键帧与其它关键帧之间的连接关系 

4.3 通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点 

4.3 检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补 

4.4 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，融合，新增或替换当前关键帧组中KF的地图点 

4.5 更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系 

4.6 结尾工作

 

---

1.函数作用

        1. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的地图点位置（相连关键帧---当前帧） 
        2. 将闭环帧以及闭环帧相连的关键帧的地图点和与当前帧相连的关键帧的点进行匹配（当前帧+相连关键帧---闭环帧+相连关键帧）     
        3. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph                 4. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整                        
        5. 创建线程进行全局Bundle Adjustment

2.函数流程 

        Step 0：结束局部地图线程、全局BA，为闭环矫正做准备
        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优化

3.code

void LoopClosing::CorrectLoop()
{

    cout << "Loop detected!" << endl;
    // Step 0：结束局部地图线程、全局BA，为闭环矫正做准备
    // 请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
    mpLocalMapper->RequestStop();

    if(isRunningGBA())
    {
        // 如果有全局BA在运行，终止掉，迎接新的全局BA
        unique_lock lock(mMutexGBA);
        mbStopGBA = true;
        // 记录全局BA次数
        mnFullBAIdx++;
        if(mpThreadGBA)
        {
            // 停止全局BA线程
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
    }

    // Wait until Local Mapping has effectively stopped
    // 一直等到局部地图线程结束再继续
    while(!mpLocalMapper->isStopped())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

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

    // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
    // Step 2：通过位姿传播，得到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 lock(mpMap->mMutexMapUpdate);

        // Step 2.1：通过mg2oScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿
        // 遍历"当前关键帧组""
        for(vector::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;
                // Pose corrected with the Sim3 of the loop closure
                // 存放闭环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);
            // Pose without correction
            // 存放没有矫正的当前关键帧的共视关键帧的Sim3变换
            NonCorrectedSim3[pKFi]=g2oSiw;
        }

        // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
        // Step 2.2：得到矫正的当前关键帧的共视关键帧位姿后，修正这些共视关键帧的地图点
        // 遍历待矫正的共视关键帧（不包括当前关键帧）
        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 vpMPsi = pKFi->GetMapPointMatches();
            // 遍历待矫正共视关键帧中的每一个地图点
            for(size_t iMP=0, endMPi = vpMPsi.size(); iMPisBad())
                    continue;
                // 标记，防止重复矫正
                if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) 
                    continue;

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

            // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
            // Step 2.3：将共视关键帧的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);

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

        // Start Loop Fusion
        // Update matched map points and replace if duplicated
        // Step 3：检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补
        // mvpCurrentMatchedPoints 是当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点
        for(size_t i=0; iGetMapPoint(i); 
                if(pCurMP)
                    // 如果有重复的MapPoint，则用匹配的地图点代替现有的
                    // 因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差
                    pCurMP->Replace(pLoopMP);
                else
                {
                    // 如果当前帧没有该MapPoint，则直接添加
                    mpCurrentKF->AddMapPoint(pLoopMP,i);
                    pLoopMP->AddObservation(mpCurrentKF,i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        } 

    }

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


    // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
    // Step 5：更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系
    // LoopConnections：存储因为闭环地图点调整而新生成的连接关系
    map > LoopConnections;

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

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

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

    // Add loop edge
    // Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
    // 它在下一次的本质图优化里面使用
    mpMatchedKF->AddLoopEdge(mpCurrentKF);
    mpCurrentKF->AddLoopEdge(mpMatchedKF);

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

    // Loop closed. Release Local Mapping.
    mpLocalMapper->Release();    

    cout << "Loop Closed!" << endl;

    mLastLoopKFid = mpCurrentKF->mnId;
}

/**
 * @brief 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，新增或替换当前关键帧组中KF的地图点
 * 因为 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的
 * 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
 * 
 * @param[in] CorrectedPosesMap         矫正的当前KF对应的共视关键帧及Sim3变换
 */
void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap)
{

    // 定义ORB匹配器
    ORBmatcher matcher(0.8);

    // Step 1 遍历待矫正的当前KF的相连关键帧
    for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
    {
        KeyFrame* pKF = mit->first;
        // 矫正过的Sim 变换
        g2o::Sim3 g2oScw = mit->second;
        cv::Mat cvScw = Converter::toCvMat(g2oScw);

        // Step 2 将mvpLoopMapPoints投影到pKF帧匹配，检查地图点冲突并融合
        // mvpLoopMapPoints：与当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点
        vector vpReplacePoints(mvpLoopMapPoints.size(),static_cast(NULL));
        // vpReplacePoints：存储mvpLoopMapPoints投影到pKF匹配后需要替换掉的新增地图点,索引和mvpLoopMapPoints一致，初始化为空
        // 搜索区域系数为4
        matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);

        // Get Map Mutex
        // 之所以不在上面 Fuse 函数中进行地图点融合更新的原因是需要对地图加锁
        unique_lock lock(mpMap->mMutexMapUpdate);
        const int nLP = mvpLoopMapPoints.size();
        // Step 3 遍历闭环帧组的所有的地图点，替换掉需要替换的地图点
        for(int i=0; iReplace(mvpLoopMapPoints[i]);
            }
        }
    }
}

4.函数解析

4.1 结束局部地图线程、全局BA，为闭环矫正做准备

        请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧。

// 外部线程调用,请求停止当前线程的工作; 其实是回环检测线程调用,来避免在进行全局优化的过程中局部建图线程添加新的关键帧
void LocalMapping::RequestStop()
{
    unique_lock lock(mMutexStop);
    mbStopRequested = true;
    unique_lock lock2(mMutexNewKFs);
    mbAbortBA = true;
}

    // 在回环纠正的时候调用,查看当前是否已经有一个全局优化的线程在进行
    bool isRunningGBA(){
        unique_lock lock(mMutexGBA);
        return mbRunningGBA;
    }

    /// 全局BA线程句柄
    std::thread* mpThreadGBA;

        如果有全局BA在运行，终止掉，迎接新的全局BA。记录全局BA次数（mnFullBAIdx）如果全局BA句柄存在则停止全局BA线程，一直等到局部建图线程结束再进行下面步骤。

4.2 根据共视关系更新当前关键帧与其它关键帧之间的连接关系 

        因为之前闭环检测、计算Sim3中改变了该关键帧的地图点，所以需要更新。

        此函数主要更新的信息是：

        @mConnectedKeyFrameWeights：当前关键帧的共视信息，记录当前关键帧共视关键帧的信息（哪一帧和当前关键帧有共视，共视程度是多少）

        @mvpOrderedConnectedKeyFrames：对mConnectedKeyFrameWeights中超过共视阈值的关键帧（按照共视程度从大到小排序）

        @mvOrderedWeights：对mConnectedKeyFrameWeights中超过共视阈值的关键帧的共视程度（按照共视程度从大到小排序）

        @mpParent：共视程度最高的那个关键帧

        @mspChildrens：建立双向关系，将当前帧的共视程度最高的那个关键帧的子关键帧添加当前帧
        具体解析请参阅我的博客：

ORB-SLAM2 --- KeyFrame::UpdateConnections 函数解析https://blog.csdn.net/qq_41694024/article/details/128516249

4.3 通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点 

        先解释几个变量：

        g2oSic： 当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的Sim3 相对变换
        mg2oScw: 世界坐标系到当前关键帧的 Sim3 变换。由ComputeSim3函数计算出来
        g2oCorrectedSiw：世界坐标系到当前关键帧共视关键帧的Sim3 变换

        这个函数的流程是这样的：

        我们在计算sim3的过程中计算出了当前帧mpCurrentKF的世界坐标系坐标，我们认为这是准确的，我们接下来要根据这个坐标修正当前帧mpCurrentKF的共视关键帧的坐标。

        我们首先将当前帧mpCurrentKF和其共视相连的关键帧加入容器mvpCurrentConnectedKFs中。对应图一的红色圈中的关键帧。

        定义了两个变量CorrectedSim3, NonCorrectedSim3：

    typedef map,       //排序算法
                Eigen::aligned_allocator > // 指定分配器,和内存空间开辟有关. 为了能够使用Eigen库中的SSE和AVX指令集加速,需要将传统STL容器中的数据进行对齐处理
                > KeyFrameAndPose;

        我们先将mpCurrentKF的Sim3变换存入，认为是准的，所以固定不动：

CorrectedSim3[mpCurrentKF]=mg2oScw;

        遍历所有共视关键帧：

        获取当前关键帧到世界坐标系下的变换矩阵Tiw：

// 获取位姿的逆
cv::Mat KeyFrame::GetPoseInverse()
{
    unique_lock lock(mMutexPose);
    return Twc.clone();
}

        是每个共视关键帧的世界坐标，我们通过下列计算求得即当前关键帧mpCurrentKF到其相连关键帧的变换矩阵。

        由于当前关键帧和其共视关键帧在同一邻域内，我们认为其没有尺度漂移，我们利用这个信息来计算准确的位姿信息：

        世界坐标系到当前帧mpCurrentKF的变换mg2oScw是准确的，我们又计算出了同一尺度下的当前帧到mpCurrentKF它的共视关键帧的变换g2oSic，我们用g2oSic*mg2oScw得到了世界到当前帧的共视关键帧的变换，我们认为这是准确的，将这个信息存储在CorrectedSim3中。

        将没有矫正过的位姿信息存放在CorrectedSim3容器中。

        到此为止，我们得到了CorrectedSim3、CorrectedSim3。

        存储着世界坐标系到当前帧坐标系的变换矩阵（修正过的、没修正过的）。

        得到矫正的当前关键帧的共视关键帧位姿后，修正这些共视关键帧的地图点：

        矫正地图点的操作如下：

        我们遍历每一个mpCurrentKF的共视关键帧pKFi：取出经过位姿传播后的Sim3变换g2oCorrectedSwi（世界坐标系到当前坐标系pKFi变换矩阵的逆矩阵），取出未经过位姿传播的Sim3变换g2oSiw（未经位姿传播的世界坐标系当当前坐标系pKFi的变换矩阵），对于帧pKFi中的每个地图点：

        我们用未修正的变换矩阵g2oSiw将地图点从世界坐标系变换到该帧pKFi的相机坐标系中，再用修正后的变换矩阵g2oCorrectedSwi将地图点从该帧的坐标系转换到世界坐标系中重新得到该地图点在世界坐标系下的坐标cvCorrectedP3Dw。        

        最后更新地图点、记录矫正该地图点的关键帧id，防止重复、记录该地图点所在的关键帧id、更新其平均观测方向以及观测距离范围。

        通过这一步，我们完成了对mpCurrentKF的共视关键帧的地图点的位姿矫正。

        现在将共视关键帧的Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿。其实是现在已经有了更新后的关键帧组中关键帧的位姿，但是在上面的操作时只是暂时存储到了 KeyFrameAndPose 类型的变量中，还没有写回到关键帧对象中。

        就是将关键帧的位姿信息更新：pKFi->SetPose(correctedTiw);

        更新关键帧的连接关系：

        更新变量

        @mConnectedKeyFrameWeights：当前关键帧的共视信息，记录当前关键帧共视关键帧的信息（哪一帧和当前关键帧有共视，共视程度是多少）

        @mvpOrderedConnectedKeyFrames：对mConnectedKeyFrameWeights中超过共视阈值的关键帧（按照共视程度从大到小排序）

        @mvOrderedWeights：对mConnectedKeyFrameWeights中超过共视阈值的关键帧的共视程度（按照共视程度从大到小排序）

        @mpParent：共视程度最高的那个关键帧

        @mspChildrens：建立双向关系，将当前帧的共视程度最高的那个关键帧的子关键帧添加当前帧
        具体讲解详见我的博客：

ORB-SLAM2 --- KeyFrame::UpdateConnections 函数解析https://blog.csdn.net/qq_41694024/article/details/128516249        因此，我们在这步中，更新了mpCurrentKF和其共视关键帧的位姿，更新了mpCurrentKF和其共视关键帧的地图点的信息。

4.3 检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补 

        mvpCurrentMatchedPoints是mpCurrentKF到其闭环匹配帧的特征点匹配关系。（经过投影匹配后修正过的）

        取出同一个索引对应的两种地图点，决定是否要替换：

        如果有重复的MapPoint，则用匹配的地图点代替现有的，因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差。

        如果当前帧没有该MapPoint，则直接添加。

4.4 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，融合，新增或替换当前关键帧组中KF的地图点 

        本步主要是将mvpLoopMapPoints中的地图点（对应图中的最终闭环关键帧和其共视关键帧的）与每一个当前帧的共视关键帧（对应图中黄色的帧CorrectedSim3）进行地图点融合。

        详见我的博客：

ORB-SLAM2 --- LoopClosing::SearchAndFuse函数解析https://blog.csdn.net/qq_41694024/article/details/128597967

4.5 更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系 

        由于我们在4.4节更新了地图点的观测，因此我们在这要对其进行校正。

        mvpCurrentConnectedKFs存储的是当前关键帧和其共视关键帧的集合。

        遍历当前帧mpCurrentKF相连关键帧组，得到与当前帧相连关键帧的相连关键帧（二级相连）存放在变量vpPreviousNeighbors中，更新一级相连关键帧的连接关系(会把当前关键帧添加进去,因为地图点已经更新和替换了)：ORB-SLAM2 --- KeyFrame::UpdateConnections 函数解析https://blog.csdn.net/qq_41694024/article/details/128516249        通过此函数更新关键帧之间的连接图。

        更新变量

        @mConnectedKeyFrameWeights：当前关键帧的共视信息，记录当前关键帧共视关键帧的信息（哪一帧和当前关键帧有共视，共视程度是多少）

        @mvpOrderedConnectedKeyFrames：对mConnectedKeyFrameWeights中超过共视阈值的关键帧（按照共视程度从大到小排序）

        @mvOrderedWeights：对mConnectedKeyFrameWeights中超过共视阈值的关键帧的共视程度（按照共视程度从大到小排序）

        @mpParent：共视程度最高的那个关键帧

        @mspChildrens：建立双向关系，将当前帧的共视程度最高的那个关键帧的子关键帧添加当前帧

// 得到与该关键帧连接（>15个共视地图点）的关键帧(没有排序的)
set KeyFrame::GetConnectedKeyFrames()
{
    unique_lock lock(mMutexConnections);

    set s;
    for(map::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++)
        s.insert(mit->first);
    return s;
}

        用变量map > LoopConnections存储因为闭环地图点调整而新生成的连接关系。

        如果更新前后有重叠的部分将其删除掉。

4.6 结尾工作

        进行本质图优化，优化本质图中所有关键帧的位姿和地图点。

        添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）。

        新建一个线程用于全局BA优化。