闭环检测与矫正

综述：

• 闭环检测与矫正代码文件： 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;
}