ORB-SLAM2 --- Tracking::NeedNewKeyFrame函数

最新推荐文章于 2024-01-14 22:48:48 发布

APS2023

最新推荐文章于 2024-01-14 22:48:48 发布

阅读量733

点赞数 1

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签： c++ slam 计算机视觉

本文链接：https://blog.csdn.net/qq_41694024/article/details/128473416

版权

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章 46 订阅

订阅专栏

1.函数作用

判断当前帧是否需要插入关键帧。

2.函数步骤

* Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧
* Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制，不插入关键帧
* Step 3：得到参考关键帧跟踪到的地图点数量
* Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧
* Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数和跟踪到的地图点数量
* Step 6：决策是否需要插入关键帧
* @return true 需要
* @return false 不需要

3.code

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;
}

4.函数解析

如果局部地图线程被闭环检测使用，则不插入关键帧。

获得当前地图的关键帧数量nKFs。

如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧。

当前帧的ID mCurrentFrame.mnId，上一次重定位帧的ID mnLastRelocFrameId，关键帧最大数目 mMaxFrames。
mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs>mMaxFrames
设置地图点的最小观测次数nMinObs，若获得当前地图的关键帧数量nKFs小于2则设置成2否则设置成3。

调用这个函数：
// 关键帧中，大于等于最少观测数目minObs的MapPoints的数量.这些特征点被认为追踪到了
int KeyFrame::TrackedMapPoints(const int &minObs)
{
    unique_lock<mutex> lock(mMutexFeatures);

    int nPoints=0;
    // 是否检查数目
    const bool bCheckObs = minObs>0;
    // N是当前帧中特征点的个数
    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = mvpMapPoints[i];
        if(pMP)     //没有被删除
        {
            if(!pMP->isBad())   //并且不是坏点
            {
                if(bCheckObs)
                {
                    // 满足输入阈值要求的地图点计数加1
                    if(mvpMapPoints[i]->Observations()>=minObs)
                        nPoints++;
                }
                else
                    nPoints++; //!bug
            }
        }
    }

    return nPoints;
}
我们遍历此帧参考关键帧中是地图点的特征点，如果此地图点被观察的次数大于我们输入阈值nMinObs，则将计数变量nPoints++。最后用nRefMatches接受计数变量。

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

        对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧。

我们决策是否需要插入关键帧！

①很长时间没有插入关键帧，可以插入--此帧的ID大于上一次插入帧的ID+最大关键帧数量（mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames）

②满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入

(mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle)

        ③在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose

④和参考帧相比当前跟踪到的点太少或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少

如果满足条件①②③其中一个且满足④：

        查询局部地图线程是否繁忙，当前能否接受新的关键帧，若能接受新的关键帧，则return true表示可以插入新的关键帧。

否则我们打断局部建图线程的BA，如果是单目且里面的关键帧没存太多则插入。否则插入不了。