详谈ORB-SLAM2的回环线程LoopClosing

ORB-SLAM2的回环线程LoopClosing分为2个步骤，非常简单。回环线程第一步是检测回环，就是因为局部建图线程使用关键帧结束后，把关键帧传给回环检测线程，所以两个之间也是有缓冲队列的。第二步就是闭环校正。

文章目录

一、回环线程成员函数/变量

1、闭环主函数: Run()

回环检测的主函数也是一个无限循环，不断检测是否出现新关键帧，缓冲队列中检测新的关键帧是否出现闭环，如果形成闭环，那就计算闭环关键帧与闭环关键帧参考的对应场景，两帧之间的匹配关系，做一个相似变换。

实际上对于双目相机来说是一个刚体变换，单目相机运行时间久导致基线消失，尺寸会漂移，所以中间会产生一个相似变换，地图里回环之后发现整体尺寸比原本的尺寸差距过大，这时候进行回环校正（CorrectLoop()）
在这里插入图片描述

void LoopClosing::Run() {
    
    
    while (1) {
    
    
        if (CheckNewKeyFrames()) {
    
    
            if (DetectLoop()) {
    
    
                if (ComputeSim3()) {
    
    
                    CorrectLoop();
                }
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

2、回环检测: DetectLoop()

在这里插入图片描述

（1）回环检测的标准

①连续3帧场景都在之前出现过
根据BOW判断
②这3帧场景在之前出现的时机也具有一定的连续性
根据连续关键帧组判断

（2）关键帧组

LoopClosing类中定义类型ConsistentGroup，表示关键帧组

typedef pair<set<KeyFrame *>, int> ConsistentGroup

第一个变量表示一组共视关键帧
第二个变量表示该关键帧组的连续长度

成员函数/变量	访问控制	意义
KeyFrame *mpCurrentKF	protected	当前关键帧
KeyFrame *mpMatchedKF	protected	当前关键帧的闭环匹配关键帧
std::vector mvConsistentGroups	protected	前一关键帧的闭环候选关键帧组
vCurrentConsistentGroups	局部变量	当前关键帧的闭环候选关键帧组
std::vector<KeyFrame *> mvpEnoughConsistentCandidates	protected	所有达到足够连续数的关键帧

（3）关键帧组闭环检测原理

若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组，且这些闭环匹配关键帧组间是连续的，则认为实现闭环
在这里插入图片描述

（4）回环检测过程

①找到当前关键帧的闭环候选关键帧vpCandidateKFs

LoopClosing缓冲队列里，输入一个关键帧，根据它的磁带向量，在之前的数据库中找到闭环候选关键帧，每个闭环候选关键帧进行扩充

闭环候选关键帧取自于与当前关键帧具有相同的BOW向量但不存在直接连接的关键帧

在这里插入图片描述

②将闭环候选关键帧和其共视关键帧组合成为关键帧组vCurrentConsistentGroups

每个闭环候选关键帧进行扩充时，将闭环候选关键帧相邻的复制关键帧构成闭环候选关键帧组，关键帧组的作用是和之前的关键帧进行匹配，比如前一帧找到了很多闭环候选关键帧组，这里的连续长度为2，已经有连续两个关键帧组跟当前关键帧组连续了，两个关键帧组之间具有同一个关键帧就算连续
在这里插入图片描述

③在当前关键组和之前的连续关键组间寻找连续关系

若当前关键帧组在之前的连续关键帧组中找到连续关系，则当前的连续关键帧组的连续长度加1
若当前关键帧组在之前的连续关键帧组中没能找到连续关系，则当前关键帧组的连续长度为0

关键帧组的连续关系是指两个关键帧组间是否有关键帧同时存在于两关键帧组中

如果组里的所有变量都找不到对应的连续性，没有能连接上的，那就称它为连续长度为0闭环候选关键帧组
在这里插入图片描述若某关键帧组的连续长度达到3，则认为该关键帧实现闭环

所以一个关键帧连续上，有连续4个关键帧对应的某个候选的关键帧组都是连续的，前一组的某一帧和后一组的某一帧是同一帧就是连续的

bool LoopClosing::DetectLoop() {
    
    
	// step1. 取出缓冲队列头部的关键帧,作为当前检测闭环关键帧,设置其不被优化删除
    {
    
    
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        mpCurrentKF->SetNotErase();
    }

    // step2. 距离上次闭环时间太短，不再检测闭环
    if (mpCurrentKF->mnId < mLastLoopKFid + 10) {
    
    
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

	// step3. 计算当前关键帧与共视关键帧间最大相似度
    const vector<KeyFrame *> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    float minScore = 1;
    for (KeyFrame *pKF : vpConnectedKeyFrames) {
    
    
        const DBoW2::BowVector &BowVec = pKF->mBowVec;
        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
        if (score < minScore)
            minScore = score;
    }

    // step4. 寻找当前关键帧的闭环候选关键帧
    vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
    if (vpCandidateKFs.empty()) {
    
    
        mpKeyFrameDB->add(mpCurrentKF);
        mvConsistentGroups.clear();
        mpCurrentKF->SetErase();
        return false;
    }
     
    // step5. 在当前关键帧组和之前的连续关键帧组之间寻找匹配
    mvpEnoughConsistentCandidates.clear();
    vector<ConsistentGroup> vCurrentConsistentGroups;
    vector<bool> vbConsistentGroup(mvConsistentGroups.size(), false);	// 之前的连续关键帧组在当前关键帧组中是否存在连续

    // 遍历当前闭环候选关键帧
    for (KeyFrame *pCandidateKF : vpCandidateKFs) {
    
    
		// step5.1. 构建关键帧组,包括候选关键帧及其共视关键帧
        set<KeyFrame *> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        spCandidateGroup.insert(pCandidateKF);

		// step5.2. 遍历之前的连续关键帧组,寻找连续关系        
        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;
            // step5.2. 若当前连续关键帧组中某关键帧也在前一帧的候选关键帧组中,则找到了连续关系
            for (KeyFrame * previousKeyFrame : spCandidateGroup.begin()) {
    
    
                if (sPreviousGroup.count(previousKeyFrame)) {
    
    
                    bConsistent = true;
                    bConsistentForSomeGroup = true;
                    break;
                }
            }

            // step5.3. 更新当前关键帧组的连续次数
            if (bConsistent) {
    
    
                int nCurrentConsistency = mvConsistentGroups[iG].second + 1;
                if (!vbConsistentGroup[iG]) {
    
    
                    ConsistentGroup cg = make_pair(spCandidateGroup, nCurrentConsistency);
                    vCurrentConsistentGroups.push_back(cg);
                    vbConsistentGroup[iG] = true;
                }
                // 若当前关键帧组的连续次数达到3,则完成闭环,将其加入到mvpEnoughConsistentCandidates中
                if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent) {
    
    
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    bEnoughConsistent = true;
                }
            }
        }
		
        // 5.4. 若当前关键帧组在前一关键帧的闭环候选关键帧组中找不到连续关系,则将两虚次数置零
        if (!bConsistentForSomeGroup) {
    
    
            ConsistentGroup cg = make_pair(spCandidateGroup, 0);
            vCurrentConsistentGroups.push_back(cg);
        }
    }
    
    // step6. 维护循环变量
    mvConsistentGroups = vCurrentConsistentGroups;		// 更新连续关键帧组
    mpKeyFrameDB->add(mpCurrentKF);						// 将当前关键帧加入到关键帧数据库中

    if (mvpEnoughConsistentCandidates.empty()) {
    
    
        mpCurrentKF->SetErase();
        return false;
    } else {
    
    
        return true;
    }
}

闭环候选关键帧的两个要求：
要求1：与当前关键帧具有相同磁带向量
要求2：与当前关键帧不直接相连

也就是说历史场景里与当前关键帧场景类似但是与当前关键帧距离很远不直接相连，这个关键帧就是闭环候选关键帧

// 寻找当前关键帧的闭环候选关键帧
vector<KeyFrame *> KeyFrameDatabase::DetectLoopCandidates(KeyFrame *pKF, float minScore) {
    
    
    
    // step1. 找出当前关键帧的所有共视关键帧
    set<KeyFrame *> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();


    // step2. 找出所有具有相同BOW但不直接相连的关键帧
    list<KeyFrame *> lKFsSharingWords;		// 存储闭环候选关键帧
    {
    
    
        unique_lock<mutex> lock(mMutex);
		// 遍历所有BOW词向量
        for (DBoW2::BowVector vit : pKF) {
    
    
            // 遍历所有含有该词向量的关键帧
            for (KeyFrame *pKFi : mvInvertedFile[vit.first]) {
    
    
                if (pKFi->mnLoopQuery != pKF->mnId) {
    
    
                    pKFi->mnLoopWords = 0;
                    // 若该关键帧与当前关键帧不直接相连,才能作为闭环候选
                    if (!spConnectedKeyFrames.count(pKFi)) {
    
    
                        pKFi->mnLoopQuery = pKF->mnId;
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                pKFi->mnLoopWords++;
            }
        }
    }

    // step3. 以最大相似度的0.8倍为阈值筛选筛选候选关键帧
    int maxCommonWords = 0;
    list<pair<float, KeyFrame *> > lScoreAndMatch;
    for (KeyFrame *pKFi : lKFsSharingWords) {
    
    
        if (*pKFi->mnLoopWords > maxCommonWords)
            maxCommonWords = *pKFi->mnLoopWords;
    }
    int minCommonWords = maxCommonWords * 0.8f;
    for (KeyFrame *pKFi : lKFsSharingWords) {
    
    
        if (pKFi->mnLoopWords > minCommonWords) {
    
    
            float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec);
            pKFi->mLoopScore = si;
            if (si >= minScore)
                lScoreAndMatch.push_back(make_pair(si, pKFi));
        }
    }

    // step4. 统计候选关键帧的共视关键帧组的相似度得分
    list<pair<float, KeyFrame *> > lAccScoreAndMatch;
    float bestAccScore = minScore;
    for (list<pair<float, KeyFrame *> >::iterator it : lScoreAndMatch) {
    
    
        KeyFrame *pKFi = it->second;
        vector<KeyFrame *> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
        float bestScore = it->first; 
        float accScore = it->first;  
        KeyFrame *pBestKF = pKFi;    
        for (vector<KeyFrame *>::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) {
    
    
            KeyFrame *pKF2 = *vit;
            if (pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords) {
    
    
                accScore += pKF2->mLoopScore;
                if (pKF2->mLoopScore > bestScore) {
    
    
                    pBestKF = pKF2;
                    bestScore = pKF2->mLoopScore;
                }
            }
        }

        lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF));
        if (accScore > bestAccScore)
            bestAccScore = accScore;
    }

	// step5. 取相似度得分高于最高相似度0.75的组的最优匹配关键帧    
    float minScoreToRetain = 0.75f * bestAccScore;
    set<KeyFrame *> spAlreadyAddedKF;
    vector<KeyFrame *> vpLoopCandidates;
    for (list<pair<float, KeyFrame *> >::iterator it : lAccScoreAndMatch.begin()) {
    
    
        if (it->first > minScoreToRetain) {
    
    
            KeyFrame *pKFi = it->second;
            if (!spAlreadyAddedKF.count(pKFi)) {
    
    
                vpLoopCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpLoopCandidates;
}

3、计算Sim3变换: ComputeSim3()

实际上ORB-SLAM2源码对双目相机和RGBD相机，实际上计算的是刚体变换，尺度不会发生变化，只有单目相机，运算时间足够长基线丢失，才会发生尺度的变化，所以单目相机就是三维相似变换。

首先，把当前帧和闭环关键帧根据词袋向量匹配，匹配成功之后，用Sim3Solver进行Sim3求解，之后进行G2O优化，优化Sim3，根据Sim3进行投影匹配，能否找到对应的地图点，如果找到足够数量的地图点那么计算成功，否则计算失败
在这里插入图片描述

bool LoopClosing::ComputeSim3() {
    
    
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
    ORBmatcher matcher(0.75, true);

    vector<Sim3Solver *> vpSim3Solvers;					// 保存每个闭环匹配关键帧的Sim3Solver
    vector<vector<MapPoint *> > vvpMapPointMatches;		// 保存当前关键帧到每个闭环匹配关键帧的匹配关系
    vector<bool> vbDiscarded;							// 保存每个闭环匹配关键帧是否是误匹配

    // step1. 为每个有超过20个匹配点的闭环关键帧创建Sim3Solver
    int nCandidates = 0;
    for (int i = 0; i < nInitialCandidates; i++) {
    
    
        KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
        pKF->SetNotErase();
        int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]);
        if (nmatches < 20) {
    
    
            vbDiscarded[i] = true;
            continue;
        } else {
    
    
            Sim3Solver *pSolver = new Sim3Solver(mpCurrentKF, pKF, vvpMapPointMatches[i], mbFixScale);
	   		pSolver->SetRansacParameters(0.99, 20, 300);
            vpSim3Solvers[i] = pSolver;
        }
        nCandidates++;
    }

	// step2. 对每个闭环候选关键帧求解优化Sim3
    bool bMatch = false;		// 是否有帧通过Sim3求解
    while (nCandidates > 0 && !bMatch) {
    
    
        for (int i = 0; i < nInitialCandidates; i++) {
    
    
            if (vbDiscarded[i])
                continue;

            KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;
            // step2.1. 进行Sim3迭代求解
            Sim3Solver *pSolver = vpSim3Solvers[i];
            cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
            if (bNoMore) {
    
    
                vbDiscarded[i] = true;
                nCandidates--;
            }

            if (!Scm.empty()) {
    
    
				// step2.2 根据计算出的Sim3搜索匹配点
                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];
                }
                cv::Mat R = pSolver->GetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();
                matcher.SearchBySim3(mpCurrentKF, pKF, vpMapPointMatches, s, R, t, 7.5);

                // step2.3. 根据搜索出的匹配点优化Sim3
                g2o::Sim3 gScm(Converter::toMatrix3d(R), Converter::toVector3d(t), s);
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
                if (nInliers >= 20) {
    
    
                    bMatch = true;
                    mpMatchedKF = pKF;
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()), Converter::toVector3d(pKF->GetTranslation()), 1.0);
                    mg2oScw = gScm * gSmw;
                    mScw = Converter::toCvMat(mg2oScw);
                    mvpCurrentMatchedPoints = vpMapPointMatches;
                    break;
                }
            }
        }
    }

    // step2.4. 优化失败,退出函数
    if (!bMatch) {
    
    
        for (int i = 0; i < nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }

    // step3. 将闭环关键帧及其共视关键帧的所有地图点 投影到 当前关键帧
    vector<KeyFrame *> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    for (KeyFrame *pKF : vpLoopConnectedKFs) {
    
    
        for (MapPoint *pMP : pKF->GetMapPointMatches()) {
    
    
            if (pMP && !pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId) {
    
    
                mvpLoopMapPoints.push_back(pMP);
                pMP->mnLoopPointForKF = mpCurrentKF->mnId;
            }
        }
    }
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints, 10);

    // step5. 根据投影成功的地图点数判断Sim3计算的是否准确
    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;
    }
}

4、闭环矫正: CorrectLoop()

在这里插入图片描述

闭环矫正一共三个步骤：

（1）Sim3位姿传播

将当前关键帧的Sim3传播到所有局部地图上，因为当前关键帧经过匹配，相当于Sim是对当前关键帧的矫正

将Sim3位姿传播到局部关键帧组上
将Sim3位姿传播到局部地图点上

（2）地图点融合

将旧的场景里面的地图点映射到当前关键帧上，进行G2O优化，然后当前关键帧的准确位姿再映射到当前关键帧组上

将闭环关键帧组地图点投影到当前关键帧上
将闭环关键帧组地图点投影到局部关键帧组上

（3）BA优化

本质图包括生成树，包括闭环产生的边，包括目视关系大于100的关键帧的边，这些构成本质图，本质图优化后，做全局BA优化，优化地图点和关键帧位姿，这个不考虑关键帧的共视关系，单纯考虑基于每个地图点到每个关键帧的投影关系，两两之间进行优化，总而言之共视图是不会优化的，只会优化本质图作为共视图的替代。

本质图BA优化，优化的是帧与帧之间的关系；全局BA优化，优化的是帧与点的关系

本质图BA优化：优化所有地图点和关键帧位姿，基于本质图
全局BA优化：优化所有地图点和关键帧位姿，基于地图点到关键帧的投影关系

void LoopClosing::CorrectLoop() {
    
    

    cout << "Loop detected!" << endl;

    // step0. 更新当前关键帧组与地图点的连接
    mpCurrentKF->UpdateConnections();

    // step1. Sim3位姿传播
    // step1.1. 构建局部关键帧组
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);
    map<KeyFrame*, g2o::Sim3> CorrectedSim3, NonCorrectedSim3;	// 存放局部关键帧组Sim3位姿传播前后的位姿
    CorrectedSim3[mpCurrentKF] = mg2oScw;    
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();
    
    {
    
    
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

        // step1.2 将Sim3位姿传播到局部关键帧组中
        for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
    
    
            cv::Mat Tiw = pKFi->GetPose();
            if (pKFi != mpCurrentKF) {
    
    
                cv::Mat Tic = Tiw * Twc;
                cv::Mat Ric = Tic.rowRange(0, 3).colRange(0, 3);
                cv::Mat tic = Tic.rowRange(0, 3).col(3);
                g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric), Converter::toVector3d(tic), 1.0);
                g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw;
                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);
            NonCorrectedSim3[pKFi] = g2oSiw;
        }
		
        // step1.3. 将Sim3位姿传播到局部地图点上
        for (pair<KeyFrame*, g2o::Sim3> mit : CorrectedSim3) {
    
    
            KeyFrame *pKFi = mit.first;
            g2o::Sim3 g2oCorrectedSiw = mit.second;
            g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
            g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];

            for (MapPoint *pMPi : pKFi->GetMapPointMatches()) {
    
    
                if (!pMPi || pMPi->isBad())
                    continue;
                if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId) 	// 标记,防止重复矫正
                    continue;
                cv::Mat P3Dw = pMPi->GetWorldPos();
                Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);
                Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
                cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                pMPi->SetWorldPos(cvCorrectedP3Dw);
                pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
                pMPi->mnCorrectedReference = pKFi->mnId;
                pMPi->UpdateNormalAndDepth();
            }

			// 将更新后的Sim3位姿赋值给关键帧变量
            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);
            pKFi->SetPose(correctedTiw);
            pKFi->UpdateConnections();
        }
	
        // step2. 地图点融合
        // step2.1 将闭环关键帧组地图点融合到当前关键帧上
        for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {
    
    
            if (mvpCurrentMatchedPoints[i]) {
    
    
                MapPoint *pLoopMP = mvpCurrentMatchedPoints[i];
                MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i);
                if (pCurMP)
                    pCurMP->Replace(pLoopMP);		// 闭环关键帧组地图点在地图中的时间更长,位姿更准确
                else {
    
    
                    mpCurrentKF->AddMapPoint(pLoopMP, i);
                    pLoopMP->AddObservation(mpCurrentKF, i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        }

    }

	// step2.2 将闭环关键帧组地图点融合到局部关键帧组上
    SearchAndFuse(CorrectedSim3);


    // step3. BA优化
    // step3.0. 查找回环连接边,用于和生成树共同组成本质图
    map<KeyFrame *, set<KeyFrame *> > LoopConnections;
    for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
    
    
        vector<KeyFrame *> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
        pKFi->UpdateConnections();
        // 闭环矫正后的共视关系 - 闭环矫正前的共视关系 = 闭环带来的新共视关系
        for (KeyFrame* prev_KFi : vpPreviousNeighbors) {
    
    
            LoopConnections[pKFi].erase(*prev_KFi);
        }
        for (KeyFrame* prev_KFi : mvpCurrentConnectedKFs) {
    
    
            LoopConnections[pKFi].erase(prev_KFi);
        }
    }

    // step3.1. 本质图BA优化
	mpMatchedKF->AddLoopEdge(mpCurrentKF);
    mpCurrentKF->AddLoopEdge(mpMatchedKF);
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

	// step3.2. 全局BA优化
    mbRunningGBA = true;
    mbFinishedGBA = false;
    mbStopGBA = false;
    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId);

    cout << "Loop Closed!" << endl;
    mLastLoopKFid = mpCurrentKF->mnId;
}

在这里插入图片描述

二、总结

sim3只针对单目，两帧之间匹配多少会有舍入误差，尺度会不断累积变大或累积变小，所以最后S肯定不会是1，S相当于补偿尺度、误匹配的计算

此博客参考ncepu_Chen的《5小时让你假装大概看懂ORB-SLAM2源码》