原创 ORB-SLAM3 LoopClosing+KeyFrameDatabase

最新推荐文章于 2024-07-25 21:56:26 发布

xiaoma_bk

最新推荐文章于 2024-07-25 21:56:26 发布

阅读量2.1k

点赞数 3

分类专栏： # ORB_SLAM3 文章标签：计算机视觉

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/121223630

版权

ORB_SLAM3 专栏收录该内容

8 篇文章 9 订阅

订阅专栏

文章目录

闭环检测与矫正
KeyFrameDatabase

闭环检测与矫正

综述：

闭环检测与矫正代码文件： LoopClosing.cc。
用到了多地图集，故闭环检测不仅在当前地图中进行，还会在以前的地图中检测。
- 若在以前的地图中检测到了回环，则在回环处进行地图的融合，并矫正融合地图中所有的关键帧位姿和地图点。
- 若是在当前地图中检测到了回环，则进行闭环矫正；

Run 闭环主函数

while 1 循环，结束后SetFinished，下面为while内执行
检测是否有新的关键帧从局部建图线程传来 CheckNewKeyFrames
return(!mlpLoopKeyFrameQueue.empty());
有新关键帧时：将当前帧在关键帧数据库中进行检测，看能否检测到闭环 NewDetectCommonRegions
- 检测到闭环时：一次判别：当前地图闭环和非当前地图闭环
- 非当前地图闭环：mbMergeDetected
  - IMU模式下只有其初始化完成后才能进行地图融合
  - 计算：匹配帧mpMergeMatchedKF与当前帧mpCurrentKF相对位姿 mSold_new
  - 如果是IMU模式：
    - 判断相对位姿尺度是否>1.1 or <0.9，如果是则放弃这次闭环，所有变量和标志位复位
    - 如果使用IMU，且GetIniertialBA1，则只更新 yaw，将 mSold_new的roll、pitch置为0
  - 地图融合与矫正。
    - IMU模式：MergeLocal2()
    - 纯视觉模式：MergeLocal()
  - 闭环结束，复位所有变量和标志，同时mbLoopDetected = false
- 当前地图闭环：mbLoopDetected
  - IMU模式时：
    - 计算当前帧到上一帧的sim3相对位姿 g2oSww_new
    - 检验一下这个相对位姿的旋转角度，roll、pitch不能大于0.008，yaw不大于0.349
    - 将roll、pitch强置为0
    - 进行闭环矫正 CorrectLoop()
  - 纯视觉模式时：
    - 直接进行闭环矫正CorrectLoop()
  - 闭环结束后，将所有的变量和标志复位
重置，如果有该请求时。ResetIfRequested
检测是否完成，完成时则 break
sleep 5ms

其他函数

NewDetectCommonRegions 检测闭环

这个函数是闭环检测线程中最重要的函数之一，用于检测闭环帧。

1、函数一开始先从mlpLoopKeyFrameQueue头部取出待检测的关键帧。
2、对该帧可进行闭环检测的一些条件判断
- 若使用IMU时，地图集需 imuBA1成功
- 若立体(双目)相机时，需地图集中关键帧个数不小于5个
- 地图集中所有关键帧个数不小于12个
3、基于连续性检测闭环，不过还没检测到第一个闭环帧时不会进入连续性检测
- 若当前地图已经检测到过闭环时：
  - 当前帧与地图匹配帧(基于上一闭环帧)及的Sim关系，评判当前帧是否可接受 DetectAndReffineSim3FromLastKF
    若其匹配数足够进行当前帧与闭环帧之间的sim3优化，且如果优化后的内点数足够，则接受该帧。
  - 接受该帧时：
    - 更新当前帧与当前地图参数：检测到闭环，次数加1，重置上一检测帧，上一检测帧地图点，未检测到个数=0
    - 若次数>=3，则确定闭环检测成功mbLoopDetected
  - 否则未接受该帧时：
    - 更新参数：未检测到闭环+未检测到个数+1
    - 若未检测到个数>=2时，清除当前地图已经检测到过标记。
- 若地图集中非当前地图已经检测到过闭环时：
  - 当前帧与地图匹配帧(基于上一闭环帧)及的Sim关系，评判当前帧是否可接受 DetectAndReffineSim3FromLastKF
    若其匹配数足够进行当前帧与闭环帧之间的sim3优化，且如果优化后的内点数足够，则接受该帧。
  - 接受该帧时：
    - 更新当前帧与当前地图参数：检测到闭环，次数加1，重置上一检测帧，上一检测帧地图点，未检测到个数=0
    - 若次数>=3，则闭环检测成功mbMergeDetected
  - 否则未接受该帧时：
    - 更新参数：未检测到闭环+未检测到个数+1
    - 若未检测到个数>=2时，清除非当前地图已经检测到过标记。
4、若当前地图闭环检测成功或地图集中非当前地图闭环检测成功有一个未检测到闭环时：
- mbMergeDetected || mbLoopDetected
- 回环队列中添加当前帧，并返回 true
5、取出当前帧的共视帧+当前帧的词袋向量，并从从词袋中提取候选者
6、当前地图未闭环或非当前地图未闭环时:
- 在当前地图和非当前地图中各找出3个与当前词袋匹配最佳的候选值 mpKeyFrameDB->DetectNBestCandidates
  - 三帧放入两队列中：vpLoopBowCand，vpMergeBowCand
7、当前地图未闭环且vpLoopBowCand非空时：
- 检测当前地图闭环，函数：mbLoopDetected=DetectCommonRegionsFromBoW
8、地图集中非当前地图未闭环且vpMergeBowCand非空时：
- 检测地图集中非当前地图闭环，函数：mbMergeDetected=DetectCommonRegionsFromBoW
9、关键帧数据中添加当前帧
10、若当前地图闭环检测成功或地图集中非当前地图闭环检测成功有一个未检测到闭环时，返回true
- mbMergeDetected || mbLoopDetected
11、一个闭环都没有，删除当前帧

代码：

bool LoopClosing::NewDetectCommonRegions()
{
    /// 步骤1：队列中取出第一个关键帧(同时将其从队列中删除)，和地图集
    {
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        // Avoid that a keyframe can be erased while it is being process by this thread
        // 设置不可删除，避免在该线程正在处理关键帧时被擦除该关键帧
        mpCurrentKF->SetNotErase();
        mpCurrentKF->mbCurrentPlaceRecognition = true;

        mpLastMap = mpCurrentKF->GetMap();
    }
	
    /// 步骤2：对该帧可进行闭环检测的一些条件判断
    
    // 2.1 若使用IMU时，地图集需 `imuBA1`成功
    if(mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA1())
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }
	
    // 2.2 若立体(双目)相机时，需地图集中关键帧个数不小于5个
    if(mpTracker->mSensor == System::STEREO && mpLastMap->GetAllKeyFrames().size() < 5) //12
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }
	
    // 2.3 地图集中所有关键帧个数不小于12个
    if(mpLastMap->GetAllKeyFrames().size() < 12)
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }
	
    /// 步骤3：基于连续性检测闭环，不过还没检测到第一个闭环帧时不会进入连续性检测
    
    //Check the last candidates with geometric validation
    bool bLoopDetectedInKF = false;
    bool bCheckSpatial = false;

    /// 如果 “当前地图” 已经检测到过 闭环时：
    if(mnLoopNumCoincidences > 0)
    {
        bCheckSpatial = true;
        // Find from the last KF candidates
        /// 计算当前帧 与地图匹配帧(基于上一闭环帧)及的Sim关系
        cv::Mat mTcl = mpCurrentKF->GetPose() * mpLoopLastCurrentKF->GetPoseInverse();
        g2o::Sim3 gScl(Converter::toMatrix3d(mTcl.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTcl.rowRange(0, 3).col(3)),1.0);
        g2o::Sim3 gScw = gScl * mg2oLoopSlw;
        int numProjMatches = 0;
        vector<MapPoint*> vpMatchedMPs;
        // 判当前帧是否可接受
        bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs);
        if(bCommonRegion) // 如果可接受
        {

            bLoopDetectedInKF = true;

            mnLoopNumCoincidences++;
            mpLoopLastCurrentKF->SetErase();
            mpLoopLastCurrentKF = mpCurrentKF;
            mg2oLoopSlw = gScw;
            mvpLoopMatchedMPs = vpMatchedMPs;

			// 若 次数>=3，则确定闭环检测成功
            mbLoopDetected = mnLoopNumCoincidences >= 3;
            mnLoopNumNotFound = 0;

            if(!mbLoopDetected)
            {
                cout << "PR: Loop detected with Reffine Sim3" << endl;
            }
        }
        else
        {
            bLoopDetectedInKF = false;
			
			// 未检测到闭环+未检测到个数+1
            mnLoopNumNotFound++;
            // 若未检测到个数>=2时，清除当前地图已经检测到过标记
            if(mnLoopNumNotFound >= 2)
            {

                mpLoopLastCurrentKF->SetErase();
                mpLoopMatchedKF->SetErase();
                mnLoopNumCoincidences = 0;
                mvpLoopMatchedMPs.clear();
                mvpLoopMPs.clear();
                mnLoopNumNotFound = 0;
            }

        }
    }

    // 若地图集中非当前地图已经检测到过闭环时：
    bool bMergeDetectedInKF = false;
    if(mnMergeNumCoincidences > 0)
    {
        // Find from the last KF candidates
        // 当前帧与地图匹配帧(基于上一闭环帧)及的Sim关系，评判当前帧是否可接受 
        cv::Mat mTcl = mpCurrentKF->GetPose() * mpMergeLastCurrentKF->GetPoseInverse();
        g2o::Sim3 gScl(Converter::toMatrix3d(mTcl.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTcl.rowRange(0, 3).col(3)),1.0);
        g2o::Sim3 gScw = gScl * mg2oMergeSlw;
        int numProjMatches = 0;
        vector<MapPoint*> vpMatchedMPs;
        bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpMergeMatchedKF, gScw, numProjMatches, mvpMergeMPs, vpMatchedMPs);
        if(bCommonRegion) // 接受该帧时：
        {
            bMergeDetectedInKF = true;

            mnMergeNumCoincidences++;
            mpMergeLastCurrentKF->SetErase();
            mpMergeLastCurrentKF = mpCurrentKF;
            mg2oMergeSlw = gScw;
            mvpMergeMatchedMPs = vpMatchedMPs;
			
			// 若 次数>=3，则 闭环检测成功mbMergeDetected
            mbMergeDetected = mnMergeNumCoincidences >= 3;
        }
        else
        {
            mbMergeDetected = false;
            bMergeDetectedInKF = false;

            mnMergeNumNotFound++;
            if(mnMergeNumNotFound >= 2)
            {

                mpMergeLastCurrentKF->SetErase();
                mpMergeMatchedKF->SetErase();
                mnMergeNumCoincidences = 0;
                mvpMergeMatchedMPs.clear();
                mvpMergeMPs.clear();
                mnMergeNumNotFound = 0;
            }


        }
    }

    /// 只要 当前地图检测到闭环 or 非当前地图检测到闭环，则添加当前帧，并返回true
    if(mbMergeDetected || mbLoopDetected)
    {
        mpKeyFrameDB->add(mpCurrentKF);
        return true;
    }
	
    /// 取出当前帧的共视帧+当前帧的词袋向量，并从从词袋中提取候选者
    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;

    // Extract candidates from the bag of words
    vector<KeyFrame*> vpMergeBowCand, vpLoopBowCand;
    
    /// 当前地图闭环 或 非当前地图闭环 有一个未检测到闭环时
    if(!bMergeDetectedInKF || !bLoopDetectedInKF)
    {
        // Search in BoW
        /// 在 *当前地图和非当前地图中* 各找出3个与当前词袋匹配最佳的候选值
        mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3);
    }

	// 当前地图中未检测到闭环且其闭环候队列非空时
    if(!bLoopDetectedInKF && !vpLoopBowCand.empty())
    {	// 检测出 当前地图 闭环
        mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs);
    }
    // Merge candidates
	// 非当前地图中未检测到闭环且其闭环候队列非空时
    if(!bMergeDetectedInKF && !vpMergeBowCand.empty())
    {	// 检测出 非当前地图 闭环
        mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs);
    }
	
    // 关键帧数据中添加当前帧
    mpKeyFrameDB->add(mpCurrentKF);
	
    /// 若 当前地图 或 非当前地图 有一个检测到闭环，则返回true
    if(mbMergeDetected || mbLoopDetected)
    {
        return true;
    }
	
    /// 一个闭环都没有，删除当前帧
    mpCurrentKF->SetErase();
    mpCurrentKF->mbCurrentPlaceRecognition = false;

    return false;
}

DetectAndReffineSim3FromLastKF 从上一关键帧中检测和精炼Sim3，确定否想关联

Use:
- bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs);
- Use Param参数：
  - mpCurrentKF 当前帧
  - mpLoopMatchedKF 闭环匹配的关键帧
  - gScw 相对关系
  - numProjMatches 当前关键帧匹配到的地图点的个数，传入0
  - mvpLoopMPs 成员变量，本次闭环涉及到的地图点
  - vpMatchedMPs 当前关键帧匹配到的地图点，每次空传进去
步骤：
- 1、基于给定的gScw按投影查找匹配点，返回匹配点个数，同时更新了所有地图点vpMPs 和已经匹配的地图点 vpMatchedMPs
- 2、若匹配点个数大于 30 时：
  - 计算新的相对关系gScm
  - 基于新的相对关系进行优化，返回优化后的匹配个数
  - 如果优化后的匹配个数大于50时：
    - 取优化后的 gScw_estimation
    - 按投影查找匹配点，返回匹配点个数，初值不一样了
    - 若匹配的地图点个数大于100，更新优化值，返回true
- 返回 false

bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
                                                 std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
{
    set<MapPoint*> spAlreadyMatchedMPs;
    // 按投影查找匹配点，返回匹配点个数，同时更新了 所有地图点vpMPs 和 已经匹配的地图点 vpMatchedMPs
    nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);


    int nProjMatches = 30;
    int nProjOptMatches = 50;
    int nProjMatchesRep = 100;

    // 若匹配点个数 > 30时
    if(nNumProjMatches >= nProjMatches) 
    {
        cv::Mat mScw = Converter::toCvMat(gScw);
        cv::Mat mTwm = pMatchedKF->GetPoseInverse();
        g2o::Sim3 gSwm(Converter::toMatrix3d(mTwm.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTwm.rowRange(0, 3).col(3)),1.0);
        // 计算新的相对关系gScm
        g2o::Sim3 gScm = gScw * gSwm;
        Eigen::Matrix<double, 7, 7> mHessian7x7;

        bool bFixedScale = mbFixScale;       // TODO CHECK; Solo para el monocular inertial
        if(mpTracker->mSensor==System::IMU_MONOCULAR && !pCurrentKF->GetMap()->GetIniertialBA2())
            bFixedScale=false;
        
        // 基于新的相对关系进行优化：Param:当前帧，匹配帧，已匹配的地图点，相对关系
        int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pMatchedKF, vpMatchedMPs, gScm, 10, bFixedScale, mHessian7x7, true);

		// 如果优化后的匹配个数大于50时
        if(numOptMatches > nProjOptMatches)	
        {
            // 取得优化后的 gScw_estimation
            g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
                           Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);

            vector<MapPoint*> vpMatchedMP;
            vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));

            // 按投影查找匹配点，返回匹配点个数，初值不一样了
            nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw_estimation, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
             // 若匹配的地图点个数大于100，更新优化值
            if(nNumProjMatches >= nProjMatchesRep)
            {
                gScw = gScw_estimation;
                return true;
            }
        }
    }
    return false;
}

FindMatchesByProjection 按投影查找匹配点，返回匹配点个数

Use:
- nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
- Use Param：
  - pCurrentKF 当前帧
  - pMatchedKF 匹配帧
  - gScw 当前帧与匹配帧的相对关系
  - spAlreadyMatchedMPs 已经匹配的地图点
  - vpMap 本次连续匹配的所有地图点
  - vpMatchedMPs 匹配地图点
步骤：
- 步骤1. 将匹配帧有最高共视的多个关键帧取出，并放入vpCovKFm
- 步骤2. 将所有匹配帧涉及到的共视帧中所有地图点取出，放入vpMapPoints和spMapPoints
- 步骤3. 基于orb匹配确定匹配地图点及个数vpMapPoints, vpMatchedMapPoints，num_matches

代码阅读：

int LoopClosing::FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,
                                         set<MapPoint*> &spMatchedMPinOrigin, vector<MapPoint*> &vpMapPoints,
                                         vector<MapPoint*> &vpMatchedMapPoints)
{
    
    /// 步骤1. 将匹配帧有最高共视的多个关键帧取出，并放入vpCovKFm
    
    int nNumCovisibles = 5;
    // 取出与匹配帧可视链接(共视地图点)最高的5个关键帧，放入`vpCovKFm`容器
    vector<KeyFrame*> vpCovKFm = pMatchedKFw->GetBestCovisibilityKeyFrames(nNumCovisibles);
    int nInitialCov = vpCovKFm.size();
    // 并将当前匹配帧也放入`vpCovKFm`容器
    vpCovKFm.push_back(pMatchedKFw);
    set<KeyFrame*> spCheckKFs(vpCovKFm.begin(), vpCovKFm.end());
    // 取出与当前帧可视链接(共视地图点)的所有关键帧
    set<KeyFrame*> spCurrentCovisbles = pCurrentKF->GetConnectedKeyFrames();
    // 遍历 匹配帧可视链接(共视地图点)最高的5个关键帧
    for(int i=0; i<nInitialCov; ++i)
    {
        // 在每个帧(匹配帧共视)中再找5个 可视链接(共视地图点)最高的帧，放入vpKFs
        vector<KeyFrame*> vpKFs = vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles);
        int nInserted = 0;
        int j = 0;
        while(j < vpKFs.size() && nInserted < nNumCovisibles)
        {	// vpKFs中既不在匹配帧共视图中，又不在当前帧共视图中，则加入匹配帧共视图中spCheckKFs
            if(spCheckKFs.find(vpKFs[j]) == spCheckKFs.end() && spCurrentCovisbles.find(vpKFs[j]) == spCurrentCovisbles.end())
            {
                spCheckKFs.insert(vpKFs[j]);
                ++nInserted;
            }
            ++j;
        }
        // 将每个帧(匹配帧共视)中5个可视链接(共视地图点)帧添加到匹配共视vpCovKFm中
        vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
    }
    set<MapPoint*> spMapPoints;
    vpMapPoints.clear();
    vpMatchedMapPoints.clear();
    
    /// 步骤2. 将所有匹配帧涉及到的共视帧中所有地图点取出，放入vpMapPoints和spMapPoints
    
    // 遍历匹配共视帧，vpCovKFm
    for(KeyFrame* pKFi : vpCovKFm)
    {	// 对每个匹配共视帧 取其匹配地图点，并遍历
        for(MapPoint* pMPij : pKFi->GetMapPointMatches())
        {	// 地图点非空且不是坏的
            if(!pMPij || pMPij->isBad())
                continue;
			// 将其地图点放入 spMapPoints 和 vpMapPoints中
            if(spMapPoints.find(pMPij) == spMapPoints.end())
            {
                spMapPoints.insert(pMPij);
                vpMapPoints.push_back(pMPij);
            }
        }
    }

    cv::Mat mScw = Converter::toCvMat(g2oScw);

    ORBmatcher matcher(0.9, true);

    vpMatchedMapPoints.resize(pCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
    /// 步骤3. 基于orb匹配确定匹配地图点及个数
    int num_matches = matcher.SearchByProjection(pCurrentKF, mScw, vpMapPoints, vpMatchedMapPoints, 3, 1.5);

    return num_matches;
}

DetectCommonRegionsFromBoW 根据bow相似度来提取候选闭环帧，然后进行匹配、优化一系列操作，判断是否闭环

Use:
- 以当前地图闭环为例，非当前地图集闭环与其类似
- mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs);
- 调用此函数的条件：
  - 1、当前关键帧在当前地图中未检测到闭环
  - 2、当前关键帧在所有关键帧中通过词袋中单词最优得到当前地图中闭环帧不为空时：
- Use Param：
  - vpLoopBowCand 当前地图闭环候选数组
  - mpLoopMatchedKF 闭环匹配帧，连续两帧未检测到时会清空
  - mpLoopLastCurrentKF 闭环检测上一关键帧，连续两帧未检测到时会清空
  - mg2oLoopSlw 与闭环检测上一关键帧的相对关系
  - mnLoopNumCoincidences 连续闭环次数记录，连续两帧未检测到时赋值为0
  - mvpLoopMPs 闭环中涉及到的地图点，连续两帧未检测到时会清空
  - mvpLoopMatchedMPs 闭环检测中匹配到的地图点，连续两帧未检测到时会清空
步骤：
- 步骤：根据bow相似度来提取候选闭环帧，然后进行匹配、优化一系列操作，判断是否闭环
  - 步骤1、取当前帧共视关键帧集合 spConnectedKeyFrames
  - 步骤2、遍历候选闭环关键帧，得到当前帧5个最佳可视图与最优候选闭环帧的个数
    
    候选闭环关键帧：闭环关键帧通过所有关键帧中找bow匹配度最高的帧
    - 1、取候选闭环帧的共视帧集合，共视程度最高的5个，同时将自身帧放入第一个数组
    - 2、遍历候选闭环帧的共视帧集合，得到最佳bow匹配的匹配个数及下标
      - 通过bow加速得到mpCurrentKF与vpCovKFi[j]之间的匹配特征点，matcherBoW.SearchByBoW
      - 记录最佳匹配的个数+下标
    - 3、遍历候选闭环帧的共视帧集合，找出已匹配上的地图点，并记录地图点匹配个数
      - 在当前帧的共视帧集合中，不是闭环，直接break本次循环
      - 遍历当前候选帧中匹配上的地图点，并放入相应容器
    - 4、若匹配地图点个数大于阈值20时，
      - 几何验证，确认当前帧与最优候选帧Sim3Solver是否收敛
        
        构造Sim3求解器如果mbFixScale为true，则是6DoFf优化（双目 RGBD），如果是false，则是7DoF优化（单目）
        
        一直循环所有的闭环候选帧，每个候选帧迭代20次，如果20次迭代后得不到结果，就换下一个候选帧
        
        直到有一个候选帧首次迭代成功bConverge和bNoMore为true
        
        最多迭代20次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
      - 已经收敛时，遍历匹配闭环帧的共视帧集合，得到匹配地图点
        
        取最佳匹配帧的5个最佳共视帧
        
        遍历匹配闭环帧的共视帧集合，得到匹配地图点
        
        得到从世界坐标系到该候选帧的Sim3变换，Scale=1，得到g2o优化后从世界坐标系到当前帧的Sim3变换
        
        将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
        
        matcher.SearchByProjection
        
        若投影匹配个数>50，则Sim3优化求解匹配帧地图点
        
        得到从世界坐标系到该候选帧的Sim3变换，Scale=1。得到g2o优化后从世界坐标系到当前帧的Sim3变换
        
        将闭环匹配上关键帧以及相连关键帧的MapPoints 再次投影到当前关键帧进行投影匹配 matcher.SearchByProjection
        
        如果这次投影地图点大于80时，默认为最佳匹配帧已经找到了
        
        取当前帧 5个最佳共视帧，循环遍历，判断当前帧的共视帧与最佳匹配帧是否想关联，并统计其个数
        
        即：5个最佳可视图与最优候选闭环帧的个数
  - 若地图匹配点个数大于0时：
    - 默认为匹配成功，并赋值参数
    - 返回：当前帧5个最佳可视图与最优候选闭环帧的个数 >=3
  - 否则，返回false

bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
                                             int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
{
    /// 该函数的作用：根据bow相似度来提取候选闭环帧，然后进行匹配、优化一系列操作，判断是否闭环

    int nBoWMatches = 20;
    int nBoWInliers = 15;
    int nSim3Inliers = 20;
    int nProjMatches = 50;
    int nProjOptMatches = 80;

    /// 步骤1. 取当前帧共视关键帧集合
    set<KeyFrame*> spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames();

    int nNumCovisibles = 5;

    /// 一些数据的定义，如：两个ORB 匹配器等
    ORBmatcher matcherBoW(0.9, true);
    ORBmatcher matcher(0.75, true);
    int nNumGuidedMatching = 0;
    KeyFrame* pBestMatchedKF;
    int nBestMatchesReproj = 0;
    int nBestNumCoindicendes = 0;
    g2o::Sim3 g2oBestScw;
    std::vector<MapPoint*> vpBestMapPoints;
    std::vector<MapPoint*> vpBestMatchedMapPoints;

    int numCandidates = vpBowCand.size();
    vector<int> vnStage(numCandidates, 0);
    vector<int> vnMatchesStage(numCandidates, 0);

    int index = 0;
    
    /// 步骤2. 遍历候选闭环关键帧(闭环关键帧通过所有关键帧中找bow匹配度最高的帧)
    for(KeyFrame* pKFi : vpBowCand)
    {
        // 关键帧是空或者坏的 直接continue
        if(!pKFi || pKFi->isBad())
            continue;

        // Current KF against KF with covisibles version
        /// 1.候选闭环帧的共视帧集合，共视程度最高的5个，同时将自身帧放入第一个数组
        std::vector<KeyFrame*> vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles);
        vpCovKFi.push_back(vpCovKFi[0]);
        vpCovKFi[0] = pKFi;

        std::vector<std::vector<MapPoint*> > vvpMatchedMPs;
        vvpMatchedMPs.resize(vpCovKFi.size());
        std::set<MapPoint*> spMatchedMPi;
        int numBoWMatches = 0;

        // 候选闭环帧 赋值 最佳bow匹配帧
        KeyFrame* pMostBoWMatchesKF = pKFi;
        int nMostBoWNumMatches = 0;

        // vpMatchedPoints，vpKeyFrameMatchedMP 构造，大小为当前帧地图点个数
        std::vector<MapPoint*> vpMatchedPoints = std::vector<MapPoint*>(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
        std::vector<KeyFrame*> vpKeyFrameMatchedMP = std::vector<KeyFrame*>(mpCurrentKF->GetMapPointMatches().size(), static_cast<KeyFrame*>(NULL));

        int nIndexMostBoWMatchesKF=0;
         /// 2.遍历候选闭环帧的共视帧集合，得到最佳bow匹配的匹配个数及下标
        for(int j=0; j<vpCovKFi.size(); ++j)
        {
            if(!vpCovKFi[j] || vpCovKFi[j]->isBad())
                continue;

            /**
             * 3d,2d都已知，计算匹配关系；通过bow，对两个关键帧的点进行匹配，用于闭环检测
             * 主要步骤：
             * 		1、两个关键帧的特征点都划分到了词袋树中不同节点中去了
             * 		2、遍历节点集合，相同的节点才计算匹配点
             * 		3、在同一节点中，遍历两个关键帧的特征点，计算描述子距离
             * 		4、描述子距离小于阈值，且次佳与最佳有一定差距，认为匹配上了
             * 		5、根据特征点angle差值构造的直方图，删除非前三的离群匹配点
             * 		6、vpMatches12保存数据，当pKF1的特征点 - pKF2的MP
             */
            // 将当前帧mpCurrentKF与闭环候选关键帧pKF匹配
            // 通过bow加速得到mpCurrentKF与vpCovKFi[j]之间的匹配特征点，vvpMatchedMPs是匹配特征点对应的MapPoints
            int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]);
            // 匹配的特征点数太少，该候选帧剔除
            if (num > nMostBoWNumMatches)
            {
                nMostBoWNumMatches = num;
                nIndexMostBoWMatchesKF = j;
            }
        }

        bool bAbortByNearKF = false;
        
        // 3.遍历候选闭环帧的共视帧集合，找出已匹配上的地图点，并记录匹配个数
        for(int j=0; j<vpCovKFi.size(); ++j)
        {
            // spConnectedKeyFrames当前帧共视，vpCovKFi闭环候选帧共视
            // 在当前帧的共视帧集合中，不是闭环，直接break本次循环
            if(spConnectedKeyFrames.find(vpCovKFi[j]) != spConnectedKeyFrames.end())
            {
                bAbortByNearKF = true;
                break;
            }
			
            // 遍历当前候选帧中匹配上的地图点
            for(int k=0; k < vvpMatchedMPs[j].size(); ++k)
            {
                MapPoint* pMPi_j = vvpMatchedMPs[j][k];
                if(!pMPi_j || pMPi_j->isBad())
                    continue;

                // 匹配地图点放入spMatchedMPi中，并赋值vpMatchedPoints+vpKeyFrameMatchedMP
                if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end())
                {
                    spMatchedMPi.insert(pMPi_j);
                    numBoWMatches++;

                    vpMatchedPoints[k]= pMPi_j;
                    vpKeyFrameMatchedMP[k] = vpCovKFi[j];
                }
            }
        }
		
        // 匹配地图点个数大于阈值20时，两帧优化`Sim3Solver`得到`mTcm`
        if(!bAbortByNearKF && numBoWMatches >= nBoWMatches) // TODO pick a good threshold
        {
            // Geometric validation  几何验证

            bool bFixedScale = mbFixScale;
            if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
                bFixedScale=false;
                        
			// 构造Sim3求解器 如果mbFixScale为true，则是6DoFf优化（双目 RGBD），如果是false，则是7DoF优化（单目）
            //param:当前帧+匹配帧+匹配地图点+固定尺度+匹配地图点(这儿二者一样)
            Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale, vpKeyFrameMatchedMP);
            solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers

            bool bNoMore = false; // 没有更多
            vector<bool> vbInliers;  
            int nInliers;
            bool bConverge = false; // 收敛
            cv::Mat mTcm;
            
            // 一直循环所有的候选帧，每个候选帧迭代20次，如果20次迭代后得不到结果，就换下一个候选帧
            // 直到有一个候选帧首次迭代成功bConverge和bNoMore为true
            while(!bConverge && !bNoMore)
            {	
                // 最多迭代20次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
                mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge);
            }

            // 已经收敛时，遍历匹配闭环帧的共视帧集合，得到匹配地图点
            if(bConverge) 
            {	
                vpCovKFi.clear();
                // 取**最佳匹配帧**的5个最佳共视帧
                vpCovKFi = pMostBoWMatchesKF->GetBestCovisibilityKeyFrames(nNumCovisibles);
                int nInitialCov = vpCovKFi.size();
                vpCovKFi.push_back(pMostBoWMatchesKF);
                set<KeyFrame*> spCheckKFs(vpCovKFi.begin(), vpCovKFi.end());

                set<MapPoint*> spMapPoints;
                vector<MapPoint*> vpMapPoints;
                vector<KeyFrame*> vpKeyFrames;
                // 遍历匹配闭环帧的共视帧集合，得到匹配地图点
                for(KeyFrame* pCovKFi : vpCovKFi)
                {	// 取出共视帧的所有匹配地图点
                    for(MapPoint* pCovMPij : pCovKFi->GetMapPointMatches())
                    {
                        if(!pCovMPij || pCovMPij->isBad())
                            continue;

                        //将地图点 及 对应的关键帧 存起来
                        if(spMapPoints.find(pCovMPij) == spMapPoints.end())
                        {
                            spMapPoints.insert(pCovMPij);
                            vpMapPoints.push_back(pCovMPij);
                            vpKeyFrames.push_back(pCovKFi);
                        }
                    }
                }

                // 得到从世界坐标系到该候选帧的Sim3变换，Scale=1
                g2o::Sim3 gScm(Converter::toMatrix3d(solver.GetEstimatedRotation()),Converter::toVector3d(solver.GetEstimatedTranslation()),solver.GetEstimatedScale());
                g2o::Sim3 gSmw(Converter::toMatrix3d(pMostBoWMatchesKF->GetRotation()),Converter::toVector3d(pMostBoWMatchesKF->GetTranslation()),1.0);
                // 得到g2o优化后从世界坐标系到当前帧的Sim3变换
                g2o::Sim3 gScw = gScm*gSmw; // Similarity matrix of current from the world position
                cv::Mat mScw = Converter::toCvMat(gScw);


                vector<MapPoint*> vpMatchedMP;
                vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
                vector<KeyFrame*> vpMatchedKF;
                vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<KeyFrame*>(NULL));
                ///  将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
                
                // 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
                // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域，
                // 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新mvpCurrentMatchedPoints
                // mvpCurrentMatchedPoints将用于SearchAndFuse中检测当前帧MapPoints与匹配的MapPoints是否存在冲突
                int numProjMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP, vpMatchedKF, 8, 1.5);
			
                // 如果匹配帧大于50时
                if(numProjMatches >= nProjMatches) //50
                {
                    // Optimize Sim3 transformation with every matches
                    Eigen::Matrix<double, 7, 7> mHessian7x7;

                    bool bFixedScale = mbFixScale;
                    if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
                        bFixedScale=false;

                    // Sim3优化求解匹配帧地图点
                    int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pKFi, vpMatchedMP, gScm, 10, mbFixScale, mHessian7x7, true);
				
                    if(numOptMatches >= nSim3Inliers) //20
                    {
                        //得到从世界坐标系到该候选帧的Sim3变换，Scale=1。得到g2o优化后从世界坐标系到当前帧的Sim3变换
                        g2o::Sim3 gSmw(Converter::toMatrix3d(pMostBoWMatchesKF->GetRotation()),Converter::toVector3d(pMostBoWMatchesKF->GetTranslation()),1.0);
                        g2o::Sim3 gScw = gScm*gSmw; // Similarity matrix of current from the world position
                        cv::Mat mScw = Converter::toCvMat(gScw);

                        vector<MapPoint*> vpMatchedMP;
                        vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
                        
                        /// 将闭环匹配上关键帧以及相连关键帧的MapPoints 再次 投影到当前关键帧进行投影匹配
                        int numProjOptMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpMatchedMP, 5, 1.0);
			
                        // 如果这次 投影 地图点大于80时，默认为最佳匹配帧已经找到了
                        if(numProjOptMatches >= nProjOptMatches) 
                        {
                            int nNumKFs = 0;
                            // Check the Sim3 transformation with the current KeyFrame covisibles
                            // 取当前帧 5个最佳共视帧
                            vector<KeyFrame*> vpCurrentCovKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(nNumCovisibles);
                            int j = 0;
                            // 循环遍历，
                            while(nNumKFs < 3 && j<vpCurrentCovKFs.size())
                            {
                                KeyFrame* pKFj = vpCurrentCovKFs[j];
                                cv::Mat mTjc = pKFj->GetPose() * mpCurrentKF->GetPoseInverse();
                                g2o::Sim3 gSjc(Converter::toMatrix3d(mTjc.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTjc.rowRange(0, 3).col(3)),1.0);
                                g2o::Sim3 gSjw = gSjc * gScw;
                                int numProjMatches_j = 0;
                                vector<MapPoint*> vpMatchedMPs_j;
                                
                                // 判断 当前帧的共视帧 与 最佳匹配帧是否 想关联
                                bool bValid = DetectCommonRegionsFromLastKF(pKFj,pMostBoWMatchesKF, gSjw,numProjMatches_j, vpMapPoints, vpMatchedMPs_j);

                                if(bValid) // 相关联则++
                                {
                                    nNumKFs++;
                                }

                                j++;
                            }

                            if(nNumKFs < 3)
                            {
                                vnStage[index] = 8;
                                vnMatchesStage[index] = nNumKFs;
                            }

                            // 若 匹配的地图点个数 小于 优化后的地图点个数，则更新匹配地图点个数等数据
                            if(nBestMatchesReproj < numProjOptMatches)
                            {
                                nBestMatchesReproj = numProjOptMatches;
                                nBestNumCoindicendes = nNumKFs; // 当前帧多少个共视帧与最佳匹配帧有关联
                                pBestMatchedKF = pMostBoWMatchesKF;
                                g2oBestScw = gScw;
                                vpBestMapPoints = vpMapPoints;
                                vpBestMatchedMapPoints = vpMatchedMP;
                            }


                        }

                    }

                }
            }
        }
        index++;
    }

    // 最佳匹配地图点个数>0 
    if(nBestMatchesReproj > 0)
    {
        pLastCurrentKF = mpCurrentKF;
        nNumCoincidences = nBestNumCoindicendes;// 当前帧多少个共视帧与最佳匹配帧有关联
        pMatchedKF2 = pBestMatchedKF;
        pMatchedKF2->SetNotErase();
        g2oScw = g2oBestScw;
        vpMPs = vpBestMapPoints;
        vpMatchedMPs = vpBestMatchedMapPoints;

        return nNumCoincidences >= 3;
    }
    else // 没有匹配的地图点，返回false
    {
        int maxStage = -1;
        int maxMatched;
        for(int i=0; i<vnStage.size(); ++i)
        {
            if(vnStage[i] > maxStage)
            {
                maxStage = vnStage[i];
                maxMatched = vnMatchesStage[i];
            }
        }

    }
    return false;
}

MergeLocal 进行地图融合，进行闭环矫正

Function：进行地图融合，进行闭环矫正。
在代码中MergeLocal( )是纯视觉时调用，MergeLocal2( )是imu模式下调用。
MergeLocal2 和MergeLocal 思路基本一致，不同在于一开始需要判断Imu是否已经完成初始化，如果没有则马上进行imu初始化再进行闭环融合，然后在融合时是将MergeMap融合到CurrMap，
步骤：
- 步骤1、若全局BA 正在运行，则终止它
- 步骤2、局部地图请求停止，usleep等待直到停止RequestStop 和 while isStopped
- 局部窗口的关键帧和当前地图的地图点合并为活跃地图。稍后，当前地图的元素转换到活跃地图上，为了实时跟踪
- 步骤3、确保当前关键帧更新了连接
- 步骤4、从“当前关键帧”地图中得到链接区域的关键帧+地图点
- 当前帧+前一帧，将其+地图点放入对应容器
- 当前帧后一帧，若存在，则将其+地图点放入对应容器
- 取当前帧“15”个最佳共视帧，放入局部关键帧窗口中
- 若共视帧小于15，且下面动作未做满3次时，则进行下列相应操作
- 步骤5、从“合并匹配帧”地图中得到链接区域的关键帧+地图点，同步骤4一样。
- 步骤6、检测融合地图，将“合并匹配帧”地图中得到链接区域的地图点放入其融合地图
- 步骤7、得到当前帧的位姿的逆
- 步骤8、遍历从“当前关键帧”地图中得到链接区域的 关键帧，并计算出与当前帧的修正关系
- 非当前帧时，计算修正参数
- 步骤9、遍历从“当前关键帧”地图中得到链接区域的 地图点，并计算出与当前帧的修正关系
- 步骤10、关键帧和地图点修正，这儿上锁，以前只取不修改
- 标记一下融合之前的位姿、其位姿的逆 + 设置位姿传播矫正后的位姿 + imu模式下还要更新速度
- 设置更新之后的坐标+ 设置平均观测方向+更新所在地图
- 在地图集中更新正在活跃中的地图 + 丢弃当前地图
- 步骤11、因为地图进行了融合，所以要重新建立关键帧之间的父子连接关系。
- 原父节点变子节点，递归
- 步骤13、更新 mpMergeMatchedKF 的连接
- 步骤14、融合局部窗口的地图点. 将在合并关键帧附近观察到的 MapPoints 投影到当前关键帧和使用校正姿势的邻居中。融合重复
- 步骤15、Update connectivity，更新所有局部关键帧的共视连接关系
- 步骤16、进行局部窗口的BA优化。
- 有无imu不同，MergeInertialBA，无imu则：LocalBundleAdjustment
- 步骤17、更新当前地图的关键帧和地图点
- 步骤18、利用位姿传播调整局部窗口以外的关键帧位姿，然后进行本质图优化(非单目情况才进行优化)
- 步骤19、全局BA。imu模式下关键帧数量必须小于200，地图集只能有一个地图，不然视觉imu全局BA计算量太大
- 步骤20、当前关键帧和合并匹配关键帧添加边
- 步骤21、地图集中移除坏地图

void LoopClosing::MergeLocal()
{
    Verbose::PrintMess("MERGE: Merge Visual detected!!!!", Verbose::VERBOSITY_NORMAL);

    int numTemporalKFs = 15;

    //Relationship to rebuild the essential graph, it is used two times, first in the local window and later in the rest of the map
    // 重建基本图的关系，使用了两次，第一次在本地窗口，然后在地图的其余部分

    KeyFrame* pNewChild;
    KeyFrame* pNewParent;

    vector<KeyFrame*> vpLocalCurrentWindowKFs;
    vector<KeyFrame*> vpMergeConnectedKFs;

    // Flag that is true only when we stopped a running BA, in this case we need relaunch at the end of the merge
    // 仅当我们停止正在运行的 BA 时才标记为真，在这种情况下，我们需要在合并结束时重新启动
    bool bRelaunchBA = false;

    Verbose::PrintMess("MERGE: Check Full Bundle Adjustment", Verbose::VERBOSITY_DEBUG);

    /// 步骤1、若全局BA 正在运行，则终止它
    // If a Global Bundle Adjustment is running, abort it
    if(isRunningGBA())
    {
        unique_lock<mutex> lock(mMutexGBA);
        mbStopGBA = true;

        mnFullBAIdx++;

        if(mpThreadGBA)
        {
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
        bRelaunchBA = true;
    }

    Verbose::PrintMess("MERGE: Request Stop Local Mapping", Verbose::VERBOSITY_DEBUG);

    /// 步骤2、局部地图请求停止，usleep等待直到停止
    mpLocalMapper->RequestStop();
    // Wait until Local Mapping has effectively stopped
    while(!mpLocalMapper->isStopped())
    {
        usleep(1000);
    }
    Verbose::PrintMess("MERGE: Local Map stopped", Verbose::VERBOSITY_DEBUG);

    mpLocalMapper->EmptyQueue();

    // Merge map will become in the new active map with the local window of KFs and MPs from the current map.
    // Later, the elements of the current map will be transform to the new active map reference, in order to keep real time tracking
    // 局部窗口的关键帧和当前地图的地图点 合并为活跃地图。稍后，当前地图的元素转换到活跃地图上，为了实时跟踪

    // 取当前帧地图 和 合并匹配帧地图
    Map* pCurrentMap = mpCurrentKF->GetMap();
    Map* pMergeMap = mpMergeMatchedKF->GetMap();

    // Ensure current keyframe is updated
    /// 步骤3、确保当前关键帧 更新了 连接
    mpCurrentKF->UpdateConnections();

    //Get the current KF and its neighbors(visual->covisibles; inertial->temporal+covisibles)
    set<KeyFrame*> spLocalWindowKFs;
    //Get MPs in the welding area from the current map

    /// 步骤4、从“当前关键帧”地图中得到 链接区域的 关键帧+地图点
    set<MapPoint*> spLocalWindowMPs;
    if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization
    {
        KeyFrame* pKFi = mpCurrentKF;
        int nInserted = 0;
        // 当前帧+前一帧，将其+地图点放入对应容器
        while(pKFi && nInserted < numTemporalKFs)
        {
            spLocalWindowKFs.insert(pKFi);
            pKFi = mpCurrentKF->mPrevKF;
            nInserted++;

            set<MapPoint*> spMPi = pKFi->GetMapPoints();
            spLocalWindowMPs.insert(spMPi.begin(), spMPi.end());
        }
        // 当前帧后一帧，若存在，则将其+地图点放入对应容器
        pKFi = mpCurrentKF->mNextKF;
        while(pKFi)
        {
            spLocalWindowKFs.insert(pKFi);

            set<MapPoint*> spMPi = pKFi->GetMapPoints();
            spLocalWindowMPs.insert(spMPi.begin(), spMPi.end());
        }
    }
    else
    {   // 若未初始化成功，则只插入：当前帧
        spLocalWindowKFs.insert(mpCurrentKF);
    }

    // 取当前帧“15”个最佳共视帧，放入 局部关键帧窗口中
    vector<KeyFrame*> vpCovisibleKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(numTemporalKFs);
    spLocalWindowKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end());
    const int nMaxTries = 3;
    int nNumTries = 0;
    // 若共视帧小于15，且下面动作未做满3次时，则进行下列相应操作
    while(spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries)
    {
        vector<KeyFrame*> vpNewCovKFs;
        vpNewCovKFs.empty();
        // 遍历局部关键帧窗口，找到7个最佳共视帧，若未在局部关键帧窗口中，则加入
        for(KeyFrame* pKFi : spLocalWindowKFs)
        {
            vector<KeyFrame*> vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2);
            for(KeyFrame* pKFcov : vpKFiCov)
            {
                if(pKFcov && !pKFcov->isBad() && spLocalWindowKFs.find(pKFcov) == spLocalWindowKFs.end())
                {
                    vpNewCovKFs.push_back(pKFcov);
                }

            }
        }

        spLocalWindowKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end());
        nNumTries++;
    }
    for(KeyFrame* pKFi : spLocalWindowKFs)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        set<MapPoint*> spMPs = pKFi->GetMapPoints();
        spLocalWindowMPs.insert(spMPs.begin(), spMPs.end());
    }

    /// 步骤5、从“合并匹配帧”地图中得到 链接区域的 关键帧+地图点
    set<KeyFrame*> spMergeConnectedKFs;

    // 先取 前后
    if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization
    {
        KeyFrame* pKFi = mpMergeMatchedKF;
        int nInserted = 0;
        while(pKFi && nInserted < numTemporalKFs)
        {
            spMergeConnectedKFs.insert(pKFi);
            pKFi = mpCurrentKF->mPrevKF;
            nInserted++;
        }

        pKFi = mpMergeMatchedKF->mNextKF;
        while(pKFi)
        {
            spMergeConnectedKFs.insert(pKFi);
        }
    }
    else
    {
        spMergeConnectedKFs.insert(mpMergeMatchedKF);
    }
    vpCovisibleKFs = mpMergeMatchedKF->GetBestCovisibilityKeyFrames(numTemporalKFs);
    spMergeConnectedKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end());
    nNumTries = 0;
    // 再取 共视，跟“当前帧局部地图”一样
    while(spMergeConnectedKFs.size() < numTemporalKFs && nNumTries < nMaxTries)
    {
        vector<KeyFrame*> vpNewCovKFs;
        for(KeyFrame* pKFi : spMergeConnectedKFs)
        {
            vector<KeyFrame*> vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2);
            for(KeyFrame* pKFcov : vpKFiCov)
            {
                if(pKFcov && !pKFcov->isBad() && spMergeConnectedKFs.find(pKFcov) == spMergeConnectedKFs.end())
                {
                    vpNewCovKFs.push_back(pKFcov);
                }

            }
        }

        spMergeConnectedKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end());
        nNumTries++;
    }

    set<MapPoint*> spMapPointMerge;
    for(KeyFrame* pKFi : spMergeConnectedKFs)
    {
        set<MapPoint*> vpMPs = pKFi->GetMapPoints();
        spMapPointMerge.insert(vpMPs.begin(),vpMPs.end());
    }


    /// 步骤6、检测融合地图，将“合并匹配帧”地图中得到 链接区域的地图点 放入其融合地图
    vector<MapPoint*> vpCheckFuseMapPoint;
    vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
    std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));

    /// 步骤7、得到当前帧的位姿的逆
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();

    cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
    cv::Mat twc = Twc.rowRange(0,3).col(3);
    g2o::Sim3 g2oNonCorrectedSwc(Converter::toMatrix3d(Rwc),Converter::toVector3d(twc),1.0);
    g2o::Sim3 g2oNonCorrectedScw = g2oNonCorrectedSwc.inverse();
    g2o::Sim3 g2oCorrectedScw = mg2oMergeScw;

    KeyFrameAndPose vCorrectedSim3, vNonCorrectedSim3;
    vCorrectedSim3[mpCurrentKF]=g2oCorrectedScw;
    vNonCorrectedSim3[mpCurrentKF]=g2oNonCorrectedScw;

    /// 步骤8、遍历从“当前关键帧”地图中得到 链接区域的 关键帧，并计算出与当前帧的修正关系
    for(KeyFrame* pKFi : spLocalWindowKFs)
    {
        if(!pKFi || pKFi->isBad())
        {
            continue;
        }

        g2o::Sim3 g2oCorrectedSiw;

        if(pKFi!=mpCurrentKF)
        {   // 非当前帧时，计算修正参数
            cv::Mat Tiw = pKFi->GetPose();
            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
            vNonCorrectedSim3[pKFi]=g2oSiw;

            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);
            g2oCorrectedSiw = g2oSic*mg2oMergeScw;
            vCorrectedSim3[pKFi]=g2oCorrectedSiw;
        }
        else
        {
            g2oCorrectedSiw = g2oCorrectedScw;
        }
        pKFi->mTcwMerge  = pKFi->GetPose();

        // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
        Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
        Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
        double s = g2oCorrectedSiw.scale();

        pKFi->mfScale = s;
        eigt *=(1./s); //[R t/s;0 1]

        cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);

        pKFi->mTcwMerge = correctedTiw;

        if(pCurrentMap->isImuInitialized())
        {
            Eigen::Matrix3d Rcor = eigR.transpose()*vNonCorrectedSim3[pKFi].rotation().toRotationMatrix();
            pKFi->mVwbMerge = Converter::toCvMat(Rcor)*pKFi->GetVelocity();
        }

    }

    // 步骤9、遍历从“当前关键帧”地图中得到 链接区域的 地图点，并计算出与当前帧的修正关系
    for(MapPoint* pMPi : spLocalWindowMPs)
    {
        if(!pMPi || pMPi->isBad())
            continue;

        KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
        g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse();
        g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref];

        // Project with non-corrected pose and project back with corrected pose
        cv::Mat P3Dw = pMPi->GetWorldPos();
        Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
        Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oNonCorrectedSiw.map(eigP3Dw));
        Eigen::Matrix3d eigR = g2oCorrectedSwi.rotation().toRotationMatrix();
        Eigen::Matrix3d Rcor = eigR * g2oNonCorrectedSiw.rotation().toRotationMatrix();

        cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);

        pMPi->mPosMerge = cvCorrectedP3Dw;
        pMPi->mNormalVectorMerge = Converter::toCvMat(Rcor) * pMPi->GetNormal();
    }

    /// 步骤10、关键帧和地图点修正，这儿上锁，以前只取不修改
    {
        unique_lock<mutex> currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
        unique_lock<mutex> mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map

        for(KeyFrame* pKFi : spLocalWindowKFs)
        {
            if(!pKFi || pKFi->isBad())
            {
                continue;
            }

            pKFi->mTcwBefMerge = pKFi->GetPose(); //标记一下融合之前的位姿
            pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
            pKFi->SetPose(pKFi->mTcwMerge);//设置位姿传播矫正后的位姿

            // Make sure connections are updated
            pKFi->UpdateMap(pMergeMap); //地图融合，当前地图变到之前的地图
            pKFi->mnMergeCorrectedForKF = mpCurrentKF->mnId;
            pMergeMap->AddKeyFrame(pKFi);
            pCurrentMap->EraseKeyFrame(pKFi);

            if(pCurrentMap->isImuInitialized())
            {
                pKFi->SetVelocity(pKFi->mVwbMerge); //imu模式下还要更新速度
            }
        }

        for(MapPoint* pMPi : spLocalWindowMPs)
        {
            if(!pMPi || pMPi->isBad())
                continue;

            pMPi->SetWorldPos(pMPi->mPosMerge); //设置更新之后的坐标
            pMPi->SetNormalVector(pMPi->mNormalVectorMerge); //设置平均观测方向
            pMPi->UpdateMap(pMergeMap);  //更新所在地图
            pMergeMap->AddMapPoint(pMPi);
            pCurrentMap->EraseMapPoint(pMPi);
        }

        mpAtlas->ChangeMap(pMergeMap);  //在地图集中更新正在活跃中的地图
        mpAtlas->SetMapBad(pCurrentMap);  //丢弃当前地图
        pMergeMap->IncreaseChangeIndex();
    }


    /// 步骤11、因为地图进行了融合，所以要重新建立关键帧之间的父子连接关系。

    //Rebuild the essential graph in the local window
    pCurrentMap->GetOriginKF()->SetFirstConnection(false);
    pNewChild = mpCurrentKF->GetParent(); // Old parent, it will be the new child of this KF
    pNewParent = mpCurrentKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent)

    // 当前帧将 mpMergeMatchedKF 改变父帧
    mpCurrentKF->ChangeParent(mpMergeMatchedKF);
    while(pNewChild )
    {
        // 当前帧的 原父节点 需要与当前帧断开，不然两个父节点了
        pNewChild->EraseChild(pNewParent); // We remove the relation between the old parent and the new for avoid loop
        KeyFrame * pOldParent = pNewChild->GetParent();
        // 原来父节点，变成子节点
        pNewChild->ChangeParent(pNewParent);

        // 父节点的父节点 变成 子节点的子节点
        pNewParent = pNewChild;
        pNewChild = pOldParent;

    }

    /// 步骤13、更新 mpMergeMatchedKF 的连接
    //Update the connections between the local window
    mpMergeMatchedKF->UpdateConnections();

    vpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
    vpMergeConnectedKFs.push_back(mpMergeMatchedKF);
    vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
    std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));

    // Project MapPoints observed in the neighborhood of the merge keyframe
    // into the current keyframe and neighbors using corrected poses.
    // Fuse duplications
    /// 步骤14、融合局部窗口的地图点. 将在合并关键帧附近观察到的 MapPoints 投影到当前关键帧和使用校正姿势的邻居中。 融合重复
    SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint);

    /// 步骤15、Update connectivity，更新所有局部关键帧的共视连接关系
    for(KeyFrame* pKFi : spLocalWindowKFs)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateConnections();
    }
    for(KeyFrame* pKFi : spMergeConnectedKFs)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateConnections();
    }

    bool bStop = false;
    Verbose::PrintMess("MERGE: Start local BA ", Verbose::VERBOSITY_DEBUG);
    vpLocalCurrentWindowKFs.clear();
    vpMergeConnectedKFs.clear();
    std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs));
    std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs));

    /// 步骤16、进行局部窗口的BA优化。
    if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO)
    {
        Optimizer::MergeInertialBA(mpLocalMapper->GetCurrKF(),mpMergeMatchedKF,&bStop, mpCurrentKF->GetMap(),vCorrectedSim3);
    }
    else
    {
        Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop);
    }

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

    Verbose::PrintMess("MERGE: Finish the LBA", Verbose::VERBOSITY_DEBUG);


    /// 步骤17、更新当前地图的关键帧和地图点
    //Update the non critical area from the current map to the merged map
    vector<KeyFrame*> vpCurrentMapKFs = pCurrentMap->GetAllKeyFrames();
    vector<MapPoint*> vpCurrentMapMPs = pCurrentMap->GetAllMapPoints();

    if(vpCurrentMapKFs.size() == 0)
    {
        Verbose::PrintMess("MERGE: There are not KFs outside of the welding area", Verbose::VERBOSITY_DEBUG);
    }
    else
    {
        Verbose::PrintMess("MERGE: Calculate the new position of the elements outside of the window", Verbose::VERBOSITY_DEBUG);
        //Apply the transformation
        {
            if(mpTracker->mSensor == System::MONOCULAR)
            {
                unique_lock<mutex> currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information

                for(KeyFrame* pKFi : vpCurrentMapKFs)
                {
                    if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
                    {
                        continue;
                    }

                    g2o::Sim3 g2oCorrectedSiw;

                    cv::Mat Tiw = pKFi->GetPose();
                    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
                    vNonCorrectedSim3[pKFi]=g2oSiw;

                    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 g2oSim(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
                    g2oCorrectedSiw = g2oSim*mg2oMergeScw;
                    vCorrectedSim3[pKFi]=g2oCorrectedSiw;

                    // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
                    Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
                    Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
                    double s = g2oCorrectedSiw.scale();

                    pKFi->mfScale = s;
                    eigt *=(1./s); //[R t/s;0 1]

                    cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);

                    pKFi->mTcwBefMerge = pKFi->GetPose();
                    pKFi->mTwcBefMerge = pKFi->GetPoseInverse();

                    pKFi->SetPose(correctedTiw);

                    if(pCurrentMap->isImuInitialized())
                    {
                        Eigen::Matrix3d Rcor = eigR.transpose()*vNonCorrectedSim3[pKFi].rotation().toRotationMatrix();
                        pKFi->SetVelocity(Converter::toCvMat(Rcor)*pKFi->GetVelocity()); // TODO: should add here scale s
                    }

                }
                for(MapPoint* pMPi : vpCurrentMapMPs)
                {
                    if(!pMPi || pMPi->isBad()|| pMPi->GetMap() != pCurrentMap)
                        continue;

                    KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
                    g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse();
                    g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref];

                    // Project with non-corrected pose and project back with corrected pose
                    cv::Mat P3Dw = pMPi->GetWorldPos();
                    Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
                    Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oNonCorrectedSiw.map(eigP3Dw));

                    cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                    pMPi->SetWorldPos(cvCorrectedP3Dw);

                    pMPi->UpdateNormalAndDepth();
                }
            }
        }

        mpLocalMapper->RequestStop();
        // Wait until Local Mapping has effectively stopped
        while(!mpLocalMapper->isStopped())
        {
            usleep(1000);
        }


        // Optimize graph (and update the loop position for each element form the begining to the end)
        /// 步骤18、利用位姿传播调整局部窗口以外的关键帧位姿，然后进行本质图优化(非单目情况才进行优化)：
        if(mpTracker->mSensor != System::MONOCULAR)
        {
            Optimizer::OptimizeEssentialGraph(mpCurrentKF, vpMergeConnectedKFs, vpLocalCurrentWindowKFs, vpCurrentMapKFs, vpCurrentMapMPs);
        }


        {
            // Get Merge Map Mutex
            unique_lock<mutex> currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
            unique_lock<mutex> mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map

            for(KeyFrame* pKFi : vpCurrentMapKFs)
            {
                if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
                {
                    continue;
                }

                // Make sure connections are updated
                pKFi->UpdateMap(pMergeMap);
                pMergeMap->AddKeyFrame(pKFi);
                pCurrentMap->EraseKeyFrame(pKFi);
            }

            for(MapPoint* pMPi : vpCurrentMapMPs)
            {
                if(!pMPi || pMPi->isBad())
                    continue;

                pMPi->UpdateMap(pMergeMap);
                pMergeMap->AddMapPoint(pMPi);
                pCurrentMap->EraseMapPoint(pMPi);
            }
        }
    }

    mpLocalMapper->Release();

    Verbose::PrintMess("MERGE:Completed!!!!!", Verbose::VERBOSITY_DEBUG);

    /// 步骤19、全局BA。imu模式下关键帧数量必须小于200，地图集只能有一个地图，不然视觉imu全局BA计算量太大
    if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1)))
    {
        // Launch a new thread to perform Global Bundle Adjustment
        Verbose::PrintMess("Relaunch Global BA", Verbose::VERBOSITY_DEBUG);
        mbRunningGBA = true;
        mbFinishedGBA = false;
        mbStopGBA = false;
        mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this, pMergeMap, mpCurrentKF->mnId);
    }

     /// 步骤20、当前关键帧 和 合并匹配关键帧 添加 边
    mpMergeMatchedKF->AddMergeEdge(mpCurrentKF);
    mpCurrentKF->AddMergeEdge(mpMergeMatchedKF);

    pCurrentMap->IncreaseChangeIndex();
    pMergeMap->IncreaseChangeIndex();

    /// 步骤21、地图集中移除坏地图
    mpAtlas->RemoveBadMaps();
}

KeyFrameDatabase

成员变量

  // Associated vocabulary
  // 预先训练好的词典
  const ORBVocabulary* mpVoc; 

  // Inverted file
  // 倒排索引，mvInvertedFile[i]表示包含了第i个word id的所有关键帧
  std::vector<list<KeyFrame*> > mvInvertedFile;

  // Mutex
  std::mutex mMutex;

部分成员函数

add 根据关键帧的词包，更新数据库的倒排索引

/**
 * @brief 根据关键帧的词包，更新数据库的倒排索引
 * @param pKF 关键帧
 */
void KeyFrameDatabase::add(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutex);

    // 为每一个word添加该KeyFrame
    for(DBoW2::BowVector::const_iterator vit= pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
        mvInvertedFile[vit->first].push_back(pKF);
}

erase 关键帧被删除后，更新数据库的倒排索引

/**
 * @brief 关键帧被删除后，更新数据库的倒排索引
 * @param pKF 关键帧
 */
void KeyFrameDatabase::erase(KeyFrame* pKF)
{
    unique_lock<mutex> lock(mMutex);

    // Erase elements in the Inverse File for the entry
    // 每一个KeyFrame包含多个words，遍历mvInvertedFile中的这些words，然后在word中删除该KeyFrame
    for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
    {
        // List of keyframes that share the word
        list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];

        for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
        {
            if(pKF==*lit)
            {
                lKFs.erase(lit);
                break;
            }
        }
    }
}

clear 清除已缓存关键帧和词典

void KeyFrameDatabase::clear()
{
    mvInvertedFile.clear();// mvInvertedFile[i]表示包含了第i个word id的所有关键帧
    mvInvertedFile.resize(mpVoc->size());// mpVoc：预先训练好的词典
}

SetORBVocabulary 设定ORB词组

void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary* pORBVoc)
{
    ORBVocabulary** ptr;
    // 赋值orb词组
    ptr = (ORBVocabulary**)( &mpVoc );
    *ptr = pORBVoc;
	
	// 清除已缓存关键帧和词典
    mvInvertedFile.clear();
    mvInvertedFile.resize(mpVoc->size());
}

DetectNBestCandidates 检测最好候选值

Use:
- mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3);
- Use Param：
  - mpCurrentKF 当前帧
  - vpLoopBowCand 当前地图闭环关键帧队列
  - vpMergeBowCand 非当前地图集闭环关键帧队列
  - 3 每类闭环帧的个数
步骤：
- 步骤1：找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）
- 步骤2：统计所有闭环候选帧中与pKF具有共同单词最多的单词数
- 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于0.7倍最大单词数的闭环帧
- 步骤4：单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
- 步骤5：闭环候选帧按照累计有效分进行排序
- 步骤6：按照累积积分，筛出所需个数的两者闭环帧

void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &vpLoopCand, vector<KeyFrame*> &vpMergeCand, int nNumCandidates)
{
    list<KeyFrame*> lKFsSharingWords;
    set<KeyFrame*> spConnectedKF;

    // Search all keyframes that share a word with current frame
    /// 步骤1：找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）
    {
        unique_lock<mutex> lock(mMutex);
		
        // 取出所有与该pKF相连的KeyFrame，这些相连Keyframe都是局部相连
        spConnectedKF = pKF->GetConnectedKeyFrames();

        // 遍历当前帧的词袋向量，words是检测图像是否匹配的枢纽，遍历该pKF的每一个word
        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
        {
            // 提取所有包含该word的KeyFrame
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
			// 遍历这些关键帧
            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                if(pKFi->mnPlaceRecognitionQuery!=pKF->mnId)// pKFi还没有标记为pKF的候选帧
                {	
                    pKFi->mnPlaceRecognitionWords=0;
                    if(!spConnectedKF.count(pKFi))// 与pKF局部链接的关键帧不进入闭环候选帧
                    {
						// pKFi标记为pKF的候选帧，之后直接跳过判断
                        pKFi->mnPlaceRecognitionQuery=pKF->mnId;
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                pKFi->mnPlaceRecognitionWords++;// 记录pKFi与pKF具有相同word的个数

            }
        }
    }
    if(lKFsSharingWords.empty()) // 若闭环候选帧为空时直接返回
        return;

    // Only compare against those keyframes that share enough words
    /// 步骤2：统计所有闭环候选帧中与pKF具有共同单词最多的单词数
    int maxCommonWords=0;
    // 遍历 所有闭环候选帧，并找出最大共同的单词个数
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnPlaceRecognitionWords>maxCommonWords)
            maxCommonWords=(*lit)->mnPlaceRecognitionWords;
    }
	// 基于最大单词个数*0.8得到 最下共同单词数
    int minCommonWords = maxCommonWords*0.8f;

    list<pair<float,KeyFrame*> > lScoreAndMatch;

    int nscores=0;

    // Compute similarity score.
    // 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于minCommonWords的闭环帧存入lScoreAndMatch
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;

        if(pKFi->mnPlaceRecognitionWords>minCommonWords)
        {
            nscores++;
            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
            pKFi->mPlaceRecognitionScore=si;
            lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }
	
    // 得分及匹配列表为空，则返回空
    if(lScoreAndMatch.empty())
        return;

    // 共有单词数大于minCommonWords的闭环帧 + 累计有效分数
    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = 0;

    
    // Lets now accumulate score by covisibility
    /// 步骤4：单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
    // 具体而言：lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch
    // 遍历 共有单词数大于minCommonWords的闭环帧
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second; 
        // 取 可视程度最高的10个相连帧
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        float bestScore = it->first;  // 该组最高分数
        float accScore = bestScore;   // 该组累计得分
        KeyFrame* pBestKF = pKFi;     // 该组最高分数对应的关键帧
        // 遍历可视程度最高的10个相连帧
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            // 若该帧未被标记为候选帧，则 跳过
            if(pKF2->mnPlaceRecognitionQuery!=pKF->mnId)
                continue;
			// 因为上面的if，所以只有pKF2也在闭环候选帧中，才能贡献分数
            accScore+=pKF2->mPlaceRecognitionScore;
             // 统计得到组里分数最高的KeyFrame
            if(pKF2->mPlaceRecognitionScore>bestScore)
            {
                pBestKF=pKF2;
                bestScore = pKF2->mPlaceRecognitionScore;
            }

        }
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        
        /// 记录所有组中组得分最高的组
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }
	
    /// 步骤5：闭环候选帧按照 累计有效分 进行排序
    lAccScoreAndMatch.sort(compFirst);

    vpLoopCand.reserve(nNumCandidates);
    vpMergeCand.reserve(nNumCandidates);
    set<KeyFrame*> spAlreadyAddedKF;
    int i = 0;
    list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin();
    
    
    /// 步骤6：按照累积积分，筛出所需个数的两者闭环帧
    
    // 遍历的条件：遍历次数小于闭环候选帧个数，且 ”当前地图“或“非当前地图”的候选闭环个数 
    while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates))
    {
        KeyFrame* pKFi = it->second;
        if(pKFi->isBad()) // 闭环候选帧是坏帧时continue
            continue;

        // 未添加到 最终闭环帧
        if(!spAlreadyAddedKF.count(pKFi)) 
        {
            // 当前地图闭环帧添加
            if(pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates)
            {
                vpLoopCand.push_back(pKFi);
            }
            // 非当前地图集 闭环帧添加
            else if(pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad())
            {
                vpMergeCand.push_back(pKFi);
            }
            spAlreadyAddedKF.insert(pKFi);
        }
        i++;
        it++;
    }
}

DetectRelocalizationCandidates 在重定位中找到与该帧相似的关键帧

步骤：
- 1、找出和当前帧具有公共单词的所有关键帧
- 2、只和具有共同单词较多的关键帧进行相似度计算
- 3、将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
- 4、只返回累计得分较高的组中分数最高的关键帧
参数：
- param F 需要重定位的帧
- return 相似的关键帧

vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F, Map* pMap)
{
    // 相对于关键帧的闭环检测DetectLoopCandidates，重定位检测中没法获得相连的关键帧
    
    // 用于保存可能与F形成回环的候选帧（只要有相同的word，且不属于局部相连帧）
    list<KeyFrame*> lKFsSharingWords;

    /// 步骤1：找出和当前帧具有公共单词的所有关键帧
         
    // Search all keyframes that share a word with current frame
    {
        unique_lock<mutex> lock(mMutex);

        // words是检测图像是否匹配的枢纽，遍历该pKF的每一个word
        for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
        {
            
            // 提取所有包含该word的KeyFrame
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                // pKFi还没有标记为pKF的候选帧
                if(pKFi->mnRelocQuery!=F->mnId) 
                {
                    pKFi->mnRelocWords=0;
                    pKFi->mnRelocQuery=F->mnId;
                    lKFsSharingWords.push_back(pKFi);
                }
                pKFi->mnRelocWords++;
            }
        }
    }
    
    // 如果没有公共单词，则return空
    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

    // 步骤2：统计所有闭环候选帧中与当前帧F具有共同单词最多的单词数，并以此决定阈值

    // Only compare against those keyframes that share enough words
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnRelocWords>maxCommonWords)
            maxCommonWords=(*lit)->mnRelocWords;
    }

    int minCommonWords = maxCommonWords*0.8f;

    list<pair<float,KeyFrame*> > lScoreAndMatch;

    int nscores=0;

    // 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于阈值minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
        
    // Compute similarity score.
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;

        // 当前帧F只和具有共同单词较多的关键帧进行比较，需要大于minCommonWords
        if(pKFi->mnRelocWords>minCommonWords)
        {
            nscores++;
            float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
            pKFi->mRelocScore=si;
            lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }
	
    // 如果公共单词中大于阈值的帧没有，则return空
    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();

    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = 0;

    // 步骤4：计算候选帧组得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain
    
    // Lets now accumulate score by covisibility
    // 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
    // 具体而言：lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second; // 关键帧
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        float bestScore = it->first;  // 该组最高分数
        float accScore = bestScore;   // 该组累计得分
        KeyFrame* pBestKF = pKFi;     // 该组最高分数对应的关键帧
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            if(pKF2->mnRelocQuery!=F->mnId)
                continue;

            accScore+=pKF2->; // 只有pKF2也在闭环候选帧中，才能贡献分数
            if(pKF2->mRelocScore>bestScore) // 统计得到组里分数最高的KeyFrame
            {
                pBestKF=pKF2; 
                bestScore = pKF2->mRelocScore;
            }

        }
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        if(accScore>bestAccScore)  // 记录所有组中组得分最高的
            bestAccScore=accScore;  // 
    }

    // 步骤5：得到组得分大于阈值的，组内得分最高的关键帧
    
    // Return all those keyframes with a score higher than 0.75*bestScore
    float minScoreToRetain = 0.75f*bestAccScore;
    set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpRelocCandidates;
    vpRelocCandidates.reserve(lAccScoreAndMatch.size());
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        const float &si = it->first;
        // 只返回累计得分大于minScoreToRetain的组中分数最高的关键帧 0.75*bestScore
        if(si>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            if (pKFi->GetMap() != pMap)
                continue;
            if(!spAlreadyAddedKF.count(pKFi))// 判断该pKFi是否已经在队列中了
            {
                vpRelocCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpRelocCandidates;
}

xiaoma_bk

关注

3
点赞
踩
7

收藏

觉得还不错? 一键收藏
0
评论
原创 ORB-SLAM3 LoopClosing+KeyFrameDatabase

文章目录闭环检测与矫正综述：Run 闭环主函数其他函数NewDetectCommonRegions 检测闭环DetectAndReffineSim3FromLastKF闭环检测与矫正综述：闭环检测与矫正代码文件： LoopClosing.cc。用到了多地图集，故闭环检测不仅在当前地图中进行，还会在以前的地图中检测。若在以前的地图中检测到了回环，则在回环处进行地图的融合，并矫正融合地图中所有的关键帧位姿和地图点。若是在当前地图中检测到了回环，则进行闭环矫正；Run 闭环主函数w
复制链接

扫一扫

专栏目录