ORBSLAM3 --- 检测闭环及融合关键帧LoopClosing::DetectCommonRegionsFromBoW几何校验函数解析

1.函数用处

2.原理介绍

2.1 定义局部窗口

2.2 计算初始相对位姿变换

2.3 Guided Matching refinement

2.4 关键帧校验

3. code分析

4. 整体流程解析

1.函数用处

无论是对于回环还是融合，都是用DetectCommonRegionsFromBoW函数完成的，不过输入的参数不一样，回环的话输入的是回环的候选关键帧vpLoopBowCand，融合的话输入的是融合的候选关键帧vpMergeBowCand。mnMergeNumCoincidences/mnLoopNumCoincidences变量是成功几何验证的帧数，超过3就认为最终验证成功，不超过3则需要继续验证。验证成功返回值mbLoopDetected、mbMergeDetected为true，反之为false则需要到第三步进行时序验证。mpLoopMatchedKF、 mpMergeMatchedKF保存最后成功匹配的候选关键帧。
    // Check the BoW candidates if the geometric candidate list is empty
    //Loop candidates
    // Step 4.1 若当前关键帧没有被检测到回环,并且候选帧数量不为0,则对回环候选帧进行论文中第8页的2-5步
    if(!bLoopDetectedInKF && !vpLoopBowCand.empty())
    {
        // mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为最终验证成功（mbLoopDetected=true），不超过继续进行时序验证
        // mpLoopMatchedKF 最后成功匹配的候选关键帧
        mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs);
    }
    // Merge candidates
    // Step 4.2 若当前关键帧没有被检测到融合,并且候选帧数量不为0,则对融合候帧进行论文中第8页的2-5步
    if(!bMergeDetectedInKF && !vpMergeBowCand.empty())
    {
        // mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为最终验证成功（mbMergeDetected=true），不超过继续进行时序验证
        mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs);
    }

2.原理介绍

2.1 定义局部窗口

        对于每个候选帧我们都定义一个局部窗口，窗口内包含了:
        1.vpCovKFi：候选帧及其前5个共视关系最好的关键帧
        2.把候选关键帧及其共视组的所有地图点记为
        3.vvpMatchedMPs:通过DBoW2找到和当前关键帧匹配的地图点

2.2 计算初始相对位姿变换

构造Sim3Solver，利用RANSAC求解候选关键帧窗口与当前关键帧的初始相对位姿 $T_{am}$
在单目或单目+IMU模式下 $T_{am}$ 就是Sim(3)；其他模式下， $T_{am}$ 就是SE(3)，后面统一用
$T_{am}$ 来表示。

2.3 Guided Matching refinement

        目的：利用上面的位姿初始值，通过投影的方式搜索更多的匹配，并优化位姿 $T_{am}$ ，具体流程：
        把通过初始相对位姿 $T_{am}$ 转换并投影到当前关键帧中，寻找更多的匹配(searhByProjection)。

        反向搜索把的地图点也通过 $T_{am}^{-1}$ 转换并投影到局部窗口里的所有关键帧上。
        利用双向搜索到的更多的匹配点非线性优化重投影误差得到更精确的相对位姿 $T_{am}$ 。
        如果内点超过一定的阈值，用更严格的匹配点搜索半径和汉明匹配距离,重新进行上述引导匹配操作。期望得到最高精度的相对位姿 $T_{am}$ 。

2.4 关键帧校验

ORB-SLAM3采用的验证模式是集卡式(一堆关键帧里面有3个可以验证的候选关键帧即可 )
对应的函数 LoopClosing::DetectCommonRegionsFromLastKF()。

3. code分析

/**
 * @brief 实现论文第8页的2-5步中的一部分功能(对后面新进来的关键帧的验证没有放在这个函数里进行)
 * 1. 构造局部窗口
 * 2. Ransac 得到 Scm的初始值
 * 3. guided matching refinement
 * 4. 利用地图中的共视关键帧验证(共视几何校验)
 * 
 * @param[in] vpBowCand bow 给出的一些候选关键帧
 * @param[out] pMatchedKF2 最后成功匹配的候选关键帧
 * @param[out] pLastCurrentKF 用于记录当前关键帧为上一个关键帧(后续若仍需要时序几何校验需要记录此信息)
 * @param[out] g2oScw 候选关键帧世界坐标系到当前关键帧的Sim3变换
 * @param[out] nNumCoincidences 成功几何验证的帧数，超过3就认为几何验证成功，不超过继续进行时序验证
 * @param[out] vpMPs  所有地图点
 * @param[out] vpMatchedMPs 成功匹配的地图点 
 * @return true 检测到一个合格的共同区域
 * @return 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)
{
    // 阀值
    int nBoWMatches = 20; // 最低bow匹配特征点数
    int nBoWInliers = 15; // RANSAC最低的匹配点数
    int nSim3Inliers = 20; // sim3 最低内点数 
    int nProjMatches = 50; // 通过投影得到的匹配点数量最低阀值
    int nProjOptMatches = 80; // 通过更小的半径,更严的距离搜索到的匹配点数量

    // 1. 获取当前帧的共视帧(在共同区域检测中应该避免当前关键帧的共视关键帧中)
    set<KeyFrame*> spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames();

    // change 定义最佳共视关键帧的数量 0.4版本这里为5
    int nNumCovisibles = 10;


    ORBmatcher matcherBoW(0.9, true);  // 用于search by bow
    ORBmatcher matcher(0.75, true);  // 用与seach by projection

    // Varibles to select the best numbe
    // 一些用于统计最优数据的变量,我们最后返回的是最佳的一个关键帧(几何校验匹配数最高的)
    KeyFrame* pBestMatchedKF;
    int nBestMatchesReproj = 0;
    int nBestNumCoindicendes = 0;
    g2o::Sim3 g2oBestScw;
    std::vector<MapPoint*> vpBestMapPoints;
    std::vector<MapPoint*> vpBestMatchedMapPoints;

    // bow中候选关键帧的数量
    int numCandidates = vpBowCand.size();

    // 这三个变量是作者为了后面打印观察记录的信息,可以忽略
    vector<int> vnStage(numCandidates, 0);
    vector<int> vnMatchesStage(numCandidates, 0);
    int index = 0;

    //Verbose::PrintMess("BoW candidates: There are " + to_string(vpBowCand.size()) + " possible candidates ", Verbose::VERBOSITY_DEBUG);
    // 2. 对每个候选关键帧都进行详细的分析
    for(KeyFrame* pKFi : vpBowCand)
    {
        if(!pKFi || pKFi->isBad())
            continue;


        // 2.1 获得候选关键帧的局部窗口 W_m (window_match)
        // 拿到候选关键帧的10个最优共视帧  nNumCovisibles=10  vpCovKFi放置其十个共视关键帧和它自己
        std::vector<KeyFrame*> vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles);
        if(vpCovKFi.empty())
        {
            std::cout << "Covisible list empty" << std::endl;

ORBSLAM3 --- 检测闭环及融合关键帧LoopClosing::DetectCommonRegionsFromBoW几何校验函数解析

1.函数用处

2.原理介绍

2.1 定义局部窗口

2.2 计算初始相对位姿变换

2.3 Guided Matching refinement

2.4 关键帧校验

3. code分析

悦读