当前位置： 首页 > article >正文

ORB-SLAM2源码学习：Tracking.cc：Tracking::NeedNewKeyFrame是否需要插入关键帧

article 2025/3/1 13:04:35

前言

这个函数实现了视觉SLAM（同步定位与地图构建）系统中的关键帧插入决策机制。关键帧的插入对于保持地图的稠密性和系统的鲁棒性至关重要。

1.函数声明 

bool Tracking::NeedNewKeyFrame()

NeedNewKeyFrame函数属于Tracking类，返回一个布尔值，表示当前帧是否需要插入为关键帧。

2.函数定义 

流程： 

1.检查是否处于纯定位模式。

if(mbOnlyTracking)
    return false;

处于纯定位模式则就不插入关键帧。

2.检查局部地图管理器是否停止或者是被要求停止。 

if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
    return false;

如果局部地图管理器 (mpLocalMapper) 已经停止或请求停止（通常由于闭环检测的触发），则暂时不插入新的关键帧。这是为了避免在闭环优化过程中地图结构发生变化，从而导致插入的关键帧不稳定。

获取当前地图中的关键帧数量用于后续的条件判断

const int nKFs = mpMap->KeyFramesInMap();

3.检查与上一次的重定位的帧间隔以及关键帧数量限制

if(mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs > mMaxFrames)
    return false;

mCurrentFrame.mnId：当前帧的ID。

mnLastRelocFrameId：最近一次重定位帧的ID。

mMaxFrames：允许的最大帧间隔。

如果当前帧距离最近一次重定位帧的间隔小于mMaxFrames，且当前地图中的关键帧数量超过 mMaxFrames，则不插入新的关键帧。这是为了避免在短时间内频繁插入关键帧，保持关键帧的稀疏性。 

4.获取参考关键帧（与当前帧共视程度最高的关键帧）中被跟踪到的地图点数量 

int nMinObs = 3;
if(nKFs <= 2)
    nMinObs = 2;
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);

当地图中的关键帧数量较少（nKFs <= 2）时，降低地图点的最小观测次数要求，以确保足够的地图点用于后续的决策。

5.查询局部地图线程是否繁忙，当前能否接受新的关键帧

bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

6. 对于双目或RGBD摄像头，统计成功跟踪的近点的数量

 int nNonTrackedClose = 0;  //双目或RGB-D中没有跟踪到的近点
    int nTrackedClose= 0;       //双目或RGB-D中成功跟踪的近点（三维点）
    if(mSensor!=System::MONOCULAR)
    {
        for(int i =0; i<mCurrentFrame.N; i++)
        {
            // 深度值在有效范围内
            if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
            {
                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                    nTrackedClose++;
                else
                    nNonTrackedClose++;
            }
        }
    }

如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧。

 // 双目或RGBD情况下：跟踪到的地图点中近点太少 同时没有跟踪到的三维点太多，可以插入关键帧了
    // 单目时，为false
    bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);

7. 决策是否需要插入关键帧

设定参考关键帧匹配比例阈值：

float thRefRatio = 0.75f;
if(nKFs < 2)
    thRefRatio = 0.4f;
if(mSensor == System::MONOCULAR)
    thRefRatio = 0.9f;

默认情况下，参考关键帧中被跟踪到的地图点数量与当前帧匹配的比例阈值为75%。

当地图中的关键帧数量较少（nKFs <= 2）时，降低阈值到40%，以增加关键帧插入的可能性。

对于单目摄像头，增加阈值到90%，因为单目系统对尺度的估计较为依赖，关键帧插入需要更加谨慎。

定义插入关键帧的条件：

const bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId + mMaxFrames;

const bool c1b = (mCurrentFrame.mnId >= mnLastKeyFrameId + mMinFrames && bLocalMappingIdle);

const bool c1c = mSensor != System::MONOCULAR &&
                (mnMatchesInliers < nRefMatches * 0.25 ||
                 bNeedToInsertClose);

const bool c2 = ((mnMatchesInliers < nRefMatches * thRefRatio || bNeedToInsertClose) && mnMatchesInliers > 15);

条件1a (c1a)：自上一次插入关键帧以来，已经经过了至少 mMaxFrames 帧。满足此条件表示已经有足够的帧间隔，可以考虑插入关键帧。

条件1b (c1b)：自上一次插入关键帧以来，已经经过了至少 mMinFrames 帧，并且局部地图管理器处于空闲状态。此条件允许在特定情况下插入关键帧，确保关键帧插入的频率不会过高，同时保证局部地图管理器能够处理新的关键帧。

条件1c (c1c)：对于双目或RGB-D摄像头，当当前帧与参考关键帧匹配的内点数量少于参考关键帧的25%，或者需要插入近距离关键帧（bNeedToInsertClose），则满足此条件。此条件用于检测跟踪质量下降的情况。

条件2 (c2)：当前帧与参考关键帧匹配的内点数量小于参考关键帧的阈值比例 (thRefRatio)，或者需要插入近距离关键帧，并且匹配的内点数量大于15。此条件进一步过滤，确保仅在跟踪质量足够的情况下插入关键帧。

综合判断是否插入关键帧:

if((c1a || c1b || c1c) && c2)
{
    if(bLocalMappingIdle)
    {
        return true;
    }
    else
    {
        mpLocalMapper->InterruptBA();
        if(mSensor != System::MONOCULAR)
        {
            if(mpLocalMapper->KeyframesInQueue() < 3)
                return true;
            else
                return false;
        }
        else
            return false;
    }
}
else
    return false;

如果局部地图管理器处于空闲状态 (bLocalMappingIdle)，则直接插入关键帧，返回 true。否则，尝试中断局部地图优化（mpLocalMapper->InterruptBA()），然后根据当前系统的传感器类型和局部地图管理器的关键帧队列长度，决定是否插入关键帧：对于双目或RGB-D摄像头，如果局部地图管理器的关键帧队列中待处理的关键帧少于3个，则允许插入关键帧。对于单目摄像头，直接不插入关键帧。单目系统的关键帧插入频率通常较高，且单目系统对尺度的依赖性更强，因此在局部地图管理器忙碌时不插入关键帧，以避免插入过多的关键帧导致系统负担过重。

完整代码 

bool Tracking::NeedNewKeyFrame()
{
    // Step 1：纯VO模式下不插入关键帧
    if(mbOnlyTracking)
        return false;

    // If Local Mapping is freezed by a Loop Closure do not insert keyframes
    // Step 2：如果局部地图线程被闭环检测使用，则不插入关键帧
    if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;
    // 获取当前地图中的关键帧数目
    const int nKFs = mpMap->KeyFramesInMap();

    // Do not insert keyframes if not enough frames have passed from last relocalisation
    // mCurrentFrame.mnId是当前帧的ID
    // mnLastRelocFrameId是最近一次重定位帧的ID
    // mMaxFrames等于图像输入的帧率
    //  Step 3：如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧
    if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs>mMaxFrames)                                     
        return false;

    // Tracked MapPoints in the reference keyframe
    // Step 4：得到参考关键帧跟踪到的地图点数量
    // UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 

    // 地图点的最小观测次数
    int nMinObs = 3;
    if(nKFs<=2)
        nMinObs=2;
    // 参考关键帧地图点中观测的数目>= nMinObs的地图点数目
    int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);

    // Local Mapping accept keyframes?
    // Step 5：查询局部地图线程是否繁忙，当前能否接受新的关键帧
    bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

    // Check how many "close" points are being tracked and how many could be potentially created.
    // Step 6：对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧
     int nNonTrackedClose = 0;  //双目或RGB-D中没有跟踪到的近点
    int nTrackedClose= 0;       //双目或RGB-D中成功跟踪的近点（三维点）
    if(mSensor!=System::MONOCULAR)
    {
        for(int i =0; i<mCurrentFrame.N; i++)
        {
            // 深度值在有效范围内
            if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
            {
                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                    nTrackedClose++;
                else
                    nNonTrackedClose++;
            }
        }
    }

    // 双目或RGBD情况下：跟踪到的地图点中近点太少 同时没有跟踪到的三维点太多，可以插入关键帧了
    // 单目时，为false
    bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);

    // Step 7：决策是否需要插入关键帧
    // Thresholds
    // Step 7.1：设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧
    float thRefRatio = 0.75f;

    // 关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低
    if(nKFs<2)
        thRefRatio = 0.4f;

    //单目情况下插入关键帧的频率很高    
    if(mSensor==System::MONOCULAR)
        thRefRatio = 0.9f;

    // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
    // Step 7.2：很长时间没有插入关键帧，可以插入
    const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;

    // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
    // Step 7.3：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入
    const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);

    // Condition 1c: tracking is weak
    // Step 7.4：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose
    const bool c1c =  mSensor!=System::MONOCULAR &&             //只考虑在双目，RGB-D的情况
                    (mnMatchesInliers<nRefMatches*0.25 ||       //当前帧和地图点匹配的数目非常少
                      bNeedToInsertClose) ;                     //需要插入

    // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
    // Step 7.5：和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少
    const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);

    if((c1a||c1b||c1c)&&c2)
    {
        // If the mapping accepts keyframes, insert keyframe.
        // Otherwise send a signal to interrupt BA
        // Step 7.6：local mapping空闲时可以直接插入，不空闲的时候要根据情况插入
        if(bLocalMappingIdle)
        {
            //可以插入关键帧
            return true;
        }
        else
        {
            mpLocalMapper->InterruptBA();
            if(mSensor!=System::MONOCULAR)
            {
                // 队列里不能阻塞太多关键帧
                // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
                // 然后localmapper再逐个pop出来插入到mspKeyFrames
                if(mpLocalMapper->KeyframesInQueue()<3)
                    //队列中的关键帧数目不是很多,可以插入
                    return true;
                else
                    //队列中缓冲的关键帧数目太多,暂时不能插入
                    return false;
            }
            else
                //对于单目情况,就直接无法插入关键帧了
                //? 为什么这里对单目情况的处理不一样?
                //回答：可能是单目关键帧相对比较密集
                return false;
        }
    }
    else
        //不满足上面的条件,自然不能插入关键帧
        return false;
}

结束语

以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。 


http://www.kler.cn/a/447699.html

相关文章：

  • Android settings命令详解
  • 科研笔记：CCS CONCEPTS
  • Spark和Hive的区别
  • 服务器上加入SFTP------(小白篇 1)
  • Bayes-GRU-Attention的数据多特征分类预测Matlab实现
  • 方正畅享全媒体新闻采编系统 screen.do SQL注入漏洞复现（附脚本）
  • 基于xiaothink对Wanyv-50M模型进行c-eval评估
  • 2024年企业中生成式 AI 的现状报告
  • wget -q -O
  • 数据结构 C/C++(实验七:排序)
  • macos 隐藏、加密磁盘、文件
  • Electric Capital 2024 年开发者报告 | 洞见未来 Web3 生态发展方向
  • 【C++——临时对象，const T】
  • Python 从入门到实战46（Matplotlib绘制常用表）
  • sentinel学习笔记1-为什么需要服务降级
  • JUC并发工具---阻塞队列
  • 加电：DETR论文阅读
  • 基于鲲鹏服务器的打砖块小游戏部署
  • 《开启微服务之旅：Spring Boot 从入门到实践》（二）
  • 三种国产大语言模型Python免费调用