ORBSLAM3 --- 检测闭环及融合关键帧LoopClosing::DetectAndReffineSim3FromLastKF时序校验函数解析

目录

1.函数作用

2.调用代码解析

3.LoopClosing::DetectAndReffineSim3FromLastKF函数解析

4.LoopClosing::FindMatchesByProjection函数解析

5. ORBmatcher::SearchByProjection重载版本解析

---

1.函数作用

        检测闭环/融合候选关键帧的时候我们先进行几何校验，几何校验不通过的话我们进行时序几何校验，流程如下：

        step1：共视几何校验(先进行)

        用当前关键帧共视帧来对候选帧进行几何校验的方式：已在地图里，无需等待。

        5个共视关键帧里有3个成功验证，则校验成功
        大于0个小于3个 --> 继续进行时序几何校验
        等于0 -->  失败， 结束验证

        step2：时序几何校验(step1未成功才进入，用后面新进来的关键帧校验 )

        用时间上连续进来的新的关键帧对候选帧进行几何校验的方式：还没有在地图里，需要等等
        step1+step2 集齐3个就校验成功
        连续两个新进来的关键帧时序校验失败，则失败，结束验证。

2.调用代码解析

    // 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->GetPose() * mpLoopLastCurrentKF->GetPoseInverse()).cast<double>();
        g2o::Sim3 gScl(mTcl.unit_quaternion(),mTcl.translation(),1.0);
        // c是左面的帧,也就是我们待闭环的帧,世界坐标系下的当前帧坐标
        g2o::Sim3 gScw = gScl * mg2oLoopSlw;
        int numProjMatches = 0;
        vector<MapPoint*> vpMatchedMPs;

        // 通过把候选帧局部窗口内的地图点向新进来的关键帧投影来验证回环检测结果,并优化Sim3位姿
        // @param in : mpCurrentKF当前关键帧 mpLoopMatchedKF闭环/融合候选关键帧
        // @param out : numProjMatches匹配的地图点数量 mvpLoopMPs存放候选帧及其共视关键帧组成的窗口里所有的地图点 vpMatchedMPs当前关键帧mpCurrentKF的匹配地图点信息为NULL和Mappiont*组成的vector
        // @param in&out : gScw：当前关键帧的世界坐标
        //   numProjMatches:match的地图点数目
        bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs);
        // 如果找到共同区域(时序验证成功一次)
        if(bCommonRegion)
        {
            //标记时序检验成功一次
            bLoopDetectedInKF = true;
            // 累计正检验的成功次数
            mnLoopNumCoincidences++;
            // 不再参与新的回环检测(融合优先级大于闭环)
            mpLoopLastCurrentKF->SetErase();
            // 将当前关键帧作为上次关键帧
            mpLoopLastCurrentKF = mpCurrentKF;
            mg2oLoopSlw = gScw;  // 记录当前优化的结果为{last T_cw}即为 T_lw
            // 记录匹配到的点
            mvpLoopMatchedMPs = vpMatchedMPs;

            // 如果验证数大于等于3则为成功回环
            mbLoopDetected = mnLoopNumCoincidences >= 3;
            // 记录失败的时序校验数为0
            mnLoopNumNotFound = 0;
            //! 这里的条件反了,不过对功能没什么影响,只是打印信息
            if(!mbLoopDetected)
            {
                cout << "PR: Loop detected with Reffine Sim3" << endl;
            }
        }
        // 如果没找到共同区域(时序验证失败一次)
        else
        {
            // 当前时序验证失败
            bLoopDetectedInKF = false;
            // 递增失败的时序验证次数
            mnLoopNumNotFound++;
            // 若果连续两帧时序验证失败则整个回环检测失败
            if(mnLoopNumNotFound >= 2)
            {
                // 失败后标记重置一些信息
                mpLoopLastCurrentKF->SetErase();
                mpLoopMatchedKF->SetErase();
                mnLoopNumCoincidences = 0;
                mvpLoopMatchedMPs.clear();
                mvpLoopMPs.clear();
                mnLoopNumNotFound = 0;
            }

        }
    }

    //Merge candidates
    bool bMergeDetectedInKF = false;
    // Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面
    // mnMergeNumCoincidences表示成功校验总次数，初始化为0
    // 会先经过后面共视几何校验，如果小于3，会进到如下判断开始时序几何校验
    if(mnMergeNumCoincidences > 0)
    {
        // Find from the last KF candidates
        // 通过上一关键帧的信息,计算新的当前帧的sim3 
        // Tcl = Tcw*Twl
        Sophus: