ORBSLAM3 --- LoopClosing::NewDetectCommonRegions检测回环关键帧/融合关键帧函数解析

目录

1.函数作用

2.代码流程解释--奇葩的代码执行顺序

3.代码详细注释

4.函数具体解析

4.1 从队列中取出一个关键帧，作为当前检测共同区域的关键帧

4.2 在某些情况下不进行共同区域检测

4.3 几何一致性校验与时序校验

4.3.1 实际执行顺序1---若当前关键帧没有被检测到回环或融合,则分别通过bow拿到当前帧最好的三个回环候选帧和融合候选帧

4.3.2 实际执行顺序2 --- 候选关键帧的挑选与几何一致性校验

4.3.3 实际执行顺序3 --- 时序校验

---

1.函数作用

        检测有没有共同区域，包括检测回环和融合匹配，为之后的地图融合和回环检测作准备。
        地图融合作用：建立更多的中长期数据关联。寻找闭环/融合候选关键帧、窗口内welding BA、本质图BA、全局BA。可以将多个子地图连接成一个精确的全局地图，极大的降低整体的位姿和地图点误差,从而获得全局一致的地图和准确的位姿估计。

闭环检测图示

地图融合图示

2.代码流程解释--奇葩的代码执行顺序

// 奇葩的代码顺序
LoopClosing::NewDetectCommonRegions(){
mnLoopNumCoincidences=0; //成功验证的总次数
mnMergeNumCoincidences=0; //成功验证的总次数
bMergeDetectedInKF = false; //某次时序验证是否成功
bLoopDetectedInKF = false;
//某次时序验证是否成功
// 实际执行顺序 3,如果2没完成才执行,2完成任务则不执行3
if(mnLoopNumCoincidences > 0){
// ...
bLoopDetectedInKF = true;
//成功进行一次时序验证
mbLoopDetected = mnLoopNumCoincidences >= 3;
// 最终成功验证
// ...
}
if(mnMergeNumCoincidences > 0){
// ...
bMergeDetectedInKF = true; //成功进行一次时序验证
mnMergeNumCoincidences++; //总验证成功次数+1
mbMergeDetected = mnMergeNumCoincidences >= 3;
// 最终成功验证
// ...
}
// 实际执行顺序 1
vector<KeyFrame*> vpMergeBowCand, vpLoopBowCand;
if(!bMergeDetectedInKF || !bLoopDetectedInKF){
DetectNBestCandidates(vpLoopBowCand, vpMergeBowCand);
}
// 实际执行顺序 2
if(!bLoopDetectedInKF && !vpLoopBowCand.empty()){
// 超过3次几何验证(mnLoopNumCoincidences>=3),就认为最终验证成功(mbLoopDetected=true),不超过继续进行时序验证
mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mnLoopNumCoincidences);
}
if(!bMergeDetectedInKF && !vpMergeBowCand.empty()){
// 超过3次几何验证(mnMergeNumCoincidences>=3),就认为最终验证成功(mbMergeDetected=true),不超过继续进行时序验
证
mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mnMergeNumCoincidences);
}
// 实际执行顺序 4,只要一种验证成功就返回true
if(mbMergeDetected || mbLoopDetected){
return true;
}
}

3.代码详细注释

/**
 * @brief 检测有没有共同区域,包括检测回环和融合匹配,sim3计算,验证
 * 对应于ORB-SLAM2里的函数DetectLoop
 * @return true 
 * @return false 
 */
bool LoopClosing::NewDetectCommonRegions()
{
    // To deactivate placerecognition. No loopclosing nor merging will be performed
    // 如果一开始就不做回环的话这里就退出了，这个线程也就名存实亡了
    if(!mbActiveLC)
        return false;

    {
        // Step 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();
    }

    // Step 2 在某些情况下不进行共同区域检测
    // 1.imu模式下还没经过第二阶段初始化则不考虑
    if(mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA2())
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

    // 2.双目模式下且当前地图关键帧数少于5则不考虑
    if(mpTracker->mSensor == System::STEREO && mpLastMap->GetAllKeyFrames().size() < 5) //12
    {
        // cout << "LoopClousure: Stereo KF inserted without check: " << mpCurrentKF->mnId << endl;
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

    // 3.当前地图关键帧少于12则不进行检测
    if(mpLastMap->GetAllKeyFrames().size() < 12)
    {
        // cout << "LoopClousure: Stereo KF inserted without check, map is small: " << mpCurrentKF->mnId << endl;
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

    //cout << "LoopClousure: Checking KF: " << mpCurrentKF->mnId << endl;
    
    //Check the last candidates with geometric validation
    // Loop candidates
    // Step 3 基于前一帧的历史信息判断是否进行时序几何校验,注意这里是基于共视几何校验失败才会运行的代码,阅读代码的时候可以先看后面
    bool bLoopDetectedInKF = false;  // 某次时序验证是否成功
    bool bCheckSpatial = false;

#ifdef REGISTER_TIMES
    std::chrono::steady_clock::time_point time_StartEstSim3_1 = std::chrono::steady_clock::now();
#endif
    // Step 3.1 回环的时序几何校验。注意初始化时mnLoopNumCoincidences=0, 所以可以先跳过看后面
    // 如果成功验证总次数大于0,即几何一致性的校验的结果,几何一致性校验超过3的话成功就不进入这里了
    if(mnLoopNumCoincidences > 0)
    {
        bCheckSpatial = true;
        // Find from the last KF candidates
        // 通过上一关键帧的信息,计算新的当前帧的sim3 
        // Tcl = Tcw * Twl
        // l是上一次参与闭环/融合的"当前:"帧，也在左图中，本帧的前面，Tcl表示上一次闭环的帧到当前帧的变换矩阵,其有优化过的gScw位姿
        Sophus::SE3d mTcl = (mpCurrentKF->GetPos