ORBSLAM3 --- 是否需要插入关键帧的判定

目录

0.在跟踪线程调用

1.是否需要插入关键帧的判定 Tracking::NeedNewKeyFrame

1.1 判断1 如果是IMU模式并且当前地图中并未完成IMU的初始化

1.2  纯VO模式下不插入关键帧

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

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

1.5  对于非单目摄像头统计内外点数目

1.6 决策是否需要插入关键帧

1.6.1 几个标志位

1.6.2 根据标志位确定是否需要关键帧

2. 向地图插入关键帧Tracking::CreateNewKeyFrame

2.1 将当前帧构造成关键帧并向其赋一些状态值

2.2 将当前关键帧设置为当前帧的参考关键帧

2.3 对于双目或RGB摄像机生成地图点

2.4 将关键帧插入地图

---

0.在跟踪线程调用

            // 判断是否需要插入关键帧
            bool bNeedKF = NeedNewKeyFrame();

            // Check if we need to insert a new keyframe
            // if(bNeedKF && bOK)

            // Step 9.4 根据条件来判断是否插入关键帧
            // 需要同时满足下面条件1和2
            // 条件1：bNeedKF=true，需要插入关键帧
            // 条件2：bOK=true跟踪成功 或 IMU模式下的RECENTLY_LOST模式且mInsertKFsLost为true
            // mInsertKFsLost 字段在yaml文件中读取,默认是true
            if(bNeedKF && (bOK || (mInsertKFsLost && mState==RECENTLY_LOST &&
                                   (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD))))
                CreateNewKeyFrame();  // 创建关键帧，对于双目或RGB-D会产生新的地图点

1.是否需要插入关键帧的判定 Tracking::NeedNewKeyFrame

1.1 判断1 如果是IMU模式并且当前地图中并未完成IMU的初始化

    // 如果是IMU模式并且当前地图中未完成IMU初始化
    if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mpAtlas->GetCurrentMap()->isImuInitialized())
    {
        // 如果是IMU模式，当前帧距离上一关键帧时间戳超过0.25s，则说明需要插入关键帧，不再进行后续判断
        if (mSensor == System::IMU_MONOCULAR && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
            return true;
        else if ((mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
            return true;
        else
            return false;
    }

        对于单目模式：

        当前帧距离上一关键帧时间戳超过0.25s，则说明需要插入关键帧，不再进行后续判断，直接返回true。

        对于双目/RGBD模式 或者 当前帧距离上一关键帧时间戳超过0.25s：

        则说明需要插入关键帧，不再进行后续判断，直接返回true。

        否则返回false。

        

1.2  纯VO模式下不插入关键帧

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

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

    // Step 2：如果局部地图线程被闭环检测使用，则不插入关键帧
    if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested()) {
        /*if(mSensor == System::MONOCULAR)
        {
            std::cout << "NeedNewKeyFrame: localmap stopped" << std::endl;
        }*/
        return false;
    }

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

    // 获取当前地图中的关键帧数目
    const int nKFs = mpAtlas->KeyFramesInMap();

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

1.5  对于非单目摄像头统计内外点数目

    // 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 && mSensor!=System::IMU_MONOCULAR)
    {
        int N = (mCurrentFrame.Nleft == -1) ? mCurrentFrame.N : mCurrentFrame.Nleft;
        for(int i =0; i0 && mCurrentFrame.mvDepth[i] closed points: " + to_string(nTrackedClose) + "; non tracked closed points: " + to_string(nNonTrackedClose), Verbose::VERBOSITY_NORMAL);// Verbose::VERBOSITY_DEBUG);
    }
    // 双目或RGBD情况下：跟踪到的地图点中近点太少 同时 没有跟踪到的三维点太多，可以插入关键帧了
    // 单目时，为false
    bool bNeedToInsertClose;
    bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);

        统计双目或者RGBD相机的每一个地图点，如果是内点的话或者深度值不小于阈值则标记为内点nTrackedClose，否则记为nNonTrackedClose。

        如果成功点数小于100且失败点数大于70，则标记为可以插入关键帧。

1.6 决策是否需要插入关键帧

1.6.1 几个标志位

        设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧。

        关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低。 thRefRatio = 0.4。

        单目情况下插入关键帧的频率很高，thRefRatio = 0.9f。

        正常情况下插入关键帧的阈值为thRefRatio = 0.75。

        几个标志位的作用：

        c1a ： mCurrentFrame.mnId >= mnLastKeyFrameId+mMaxFrames（很长时间没有插入关键帧，可以插入）

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

        c1c：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少（mnMatchesInliers

        c2：和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少（(((mnMatchesInliers15);）

        c3：单目/双目/RGBD+IMU模式下，并且IMU完成了初始化（隐藏条件），当前帧和上一关键帧之间时间超过0.5秒，则c3=true。

        c4：单目+IMU模式下，当前帧匹配内点数在15~75之间或者是RECENTLY_LOST状态，c4=true。

1.6.2 根据标志位确定是否需要关键帧

    // 相比ORB-SLAM2多了c3,c4
    if(((c1a||c1b||c1c) && c2)||c3 ||c4)
    {
        // If the mapping accepts keyframes, insert keyframe.
        // Otherwise send a signal to interrupt BA
        // Step 7.6：local mapping空闲时或者正在做imu初始化时可以直接插入，不空闲的时候要根据情况插入
        if(bLocalMappingIdle || mpLocalMapper->IsInitializing())
        {
            // 可以插入关键帧
            return true;
        }
        else
        {
            mpLocalMapper->InterruptBA();
            if(mSensor!=System::MONOCULAR  && mSensor!=System::IMU_MONOCULAR)
            {
                // 双目或双目+IMU或RGB-D模式下，如队列里没有阻塞太多关键帧，可以插入
                // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
                // 然后localmapper再逐个pop出来插入到mspKeyFrames
                if(mpLocalMapper->KeyframesInQueue()<3)
                    // 队列中的关键帧数目不是很多,可以插入
                    return true;
                else
                    // 队列中缓冲的关键帧数目太多,暂时不能插入
                    return false;
            }
            else
            {
                //std::cout << "NeedNewKeyFrame: localmap is busy" << std::endl;
                //对于单目情况,就直接无法插入关键帧了
                //? 为什么这里对单目情况的处理不一样?
                //回答：可能是单目关键帧相对比较密集
                return false;
            }
        }
    }
    else
        // 不满足上面的条件,自然不能插入关键帧
        return false;

2. 向地图插入关键帧Tracking::CreateNewKeyFrame

2.1 将当前帧构造成关键帧并向其赋一些状态值

    // Step 1：将当前帧构造成关键帧
    KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);

    if(mpAtlas->isImuInitialized()) //  || mpLocalMapper->IsInitializing())
        pKF->bImu = true;

    pKF->SetNewBias(mCurrentFrame.mImuBias);

        向当前地图mpAtlas->GetCurrentMap()构造关键帧。

        如果当前地图IMU初始化完毕，则设置IMU初始化成功变量pKF->bImu为true。

        设置当前帧的IMU的偏置值pKF->SetNewBias为上一帧的偏置值。

2.2 将当前关键帧设置为当前帧的参考关键帧

    // Step 2：将当前关键帧设置为当前帧的参考关键帧
    // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
    mpReferenceKF = pKF;
    mCurrentFrame.mpReferenceKF = pKF;

    if(mpLastKeyFrame)
    {
        pKF->mPrevKF = mpLastKeyFrame;
        mpLastKeyFrame->mNextKF = pKF;
    }
    else
        Verbose::PrintMess("No last KF in KF creation!!", Verbose::VERBOSITY_NORMAL);

    // Reset preintegration from last KF (Create new object)
    if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
    {
        mpImuPreintegratedFromLastKF = new IMU::Preintegrated(pKF->GetImuBias(),pKF->mImuCalib);
    }

        设置一些承上启下的变量：

        mpReferenceKF：我理解为保存最新的关键帧

        mCurrentFrame.mpReferenceKF：由于我们进行位姿优化是对关键帧进行优化的，因此非关键帧如何获取更精准的位姿呢：通过关键帧，这里就是这个作用的。在恒速追踪模型中就有提到。

        建立图关系。

        构造IMU参数类。

2.3 对于双目或RGB摄像机生成地图点

    // 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同
    // Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作
    if(mSensor!=System::MONOCULAR && mSensor != System::IMU_MONOCULAR) // TODO check if incluide imu_stereo
    {
        // 根据Tcw计算mRcw、mtcw和mRwc、mOw
        mCurrentFrame.UpdatePoseMatrices();
        // cout << "create new MPs" << endl;
        // We sort points by the measured depth by the stereo/RGBD sensor.
        // We create all those MapPoints whose depth < mThDepth.
        // If there are less than 100 close points we create the 100 closest.
        int maxPoint = 100;
        if(mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
            maxPoint = 100;

        // Step 3.1：得到当前帧有深度值的特征点（不一定是地图点）
        vector > vDepthIdx;
        int N = (mCurrentFrame.Nleft != -1) ? mCurrentFrame.Nleft : mCurrentFrame.N;
        vDepthIdx.reserve(mCurrentFrame.N);
        for(int i=0; i0)
            {
                // 第一个元素是深度,第二个元素是对应的特征点的id
                vDepthIdx.push_back(make_pair(z,i));
            }
        }

        if(!vDepthIdx.empty())
        {
            // Step 3.2：按照深度从小到大排序
            sort(vDepthIdx.begin(),vDepthIdx.end());

            // Step 3.3：从中找出不是地图点的生成临时地图点 
            // 处理的近点的个数
            int nPoints = 0;
            for(size_t j=0; jObservations()<1)
                {
                    bCreateNew = true;
                    mCurrentFrame.mvpMapPoints[i] = static_cast(NULL);
                }

                // 如果需要就新建地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪
                if(bCreateNew)
                {
                    Eigen::Vector3f x3D;

                    if(mCurrentFrame.Nleft == -1){
                        mCurrentFrame.UnprojectStereo(i, x3D);
                    }
                    else{
                        x3D = mCurrentFrame.UnprojectStereoFishEye(i);
                    }

                    MapPoint* pNewMP = new MapPoint(x3D,pKF,mpAtlas->GetCurrentMap());
                    // 这些添加属性的操作是每次创建MapPoint后都要做的
                    pNewMP->AddObservation(pKF,i);

                    //Check if it is a stereo observation in order to not
                    //duplicate mappoints
                    if(mCurrentFrame.Nleft != -1 && mCurrentFrame.mvLeftToRightMatch[i] >= 0){
                        mCurrentFrame.mvpMapPoints[mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]]=pNewMP;
                        pNewMP->AddObservation(pKF,mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]);
                        pKF->AddMapPoint(pNewMP,mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]);
                    }

                    pKF->AddMapPoint(pNewMP,i);
                    pNewMP->ComputeDistinctiveDescriptors();
                    pNewMP->UpdateNormalAndDepth();
                    mpAtlas->AddMapPoint(pNewMP);

                    mCurrentFrame.mvpMapPoints[i]=pNewMP;
                    nPoints++;
                }
                else
                {
                    // 因为从近到远排序，记录其中不需要创建地图点的个数
                    nPoints++;
                }

                // Step 3.4：停止新建地图点必须同时满足以下条件：
                // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
                // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
                if(vDepthIdx[j].first>mThDepth && nPoints>maxPoint)
                {
                    break;
                }
            }
            //Verbose::PrintMess("new mps for stereo KF: " + to_string(nPoints), Verbose::VERBOSITY_NORMAL);
        }
    }

2.4 将关键帧插入地图

    // Step 4：插入关键帧
    // 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸
    mpLocalMapper->InsertKeyFrame(pKF);

    // 插入好了，允许局部建图停止
    mpLocalMapper->SetNotStop(false);

    // 当前帧成为新的关键帧，更新
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKF;

        注释已经写的很明白啦。