ORB-SLAM2 ---- Tracking::UpdateLastFrame函数

最新推荐文章于 2024-05-23 09:29:54 发布

APS2023

最新推荐文章于 2024-05-23 09:29:54 发布

阅读量800

点赞数 2

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

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

版权

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

66 篇文章 46 订阅

订阅专栏

4.1 利用参考关键帧更新上一帧在世界坐标系下的位姿

4.2 对于双目或rgbd相机，为上一帧生成新的临时地图点

1.函数作用

更新上一帧位姿，在上一帧中生成临时地图点。

单目情况：只计算了上一帧的世界坐标系位姿
双目和rgbd情况：选取有有深度值的并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性

2.步骤

Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿

Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点

Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）

Step 2.2：从中找出不是地图点的部分

Step 2.3：需要创建的点，包装为地图点。只是为了提高双目和RGBD的跟踪成功率，并没有添加复杂属性，因为后面会扔掉

Step 2.4：如果地图点质量不好，停止创建地图点

3.code

void Tracking::UpdateLastFrame()
{
    // Update pose according to reference keyframe
    // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
    // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧
    KeyFrame* pRef = mLastFrame.mpReferenceKF;  
    // ref_keyframe 到 lastframe的位姿变换
    cv::Mat Tlr = mlRelativeFramePoses.back();

    // 将上一帧的世界坐标系下的位姿计算出来
    // l:last, r:reference, w:world
    // Tlw = Tlr*Trw 
    mLastFrame.SetPose(Tlr*pRef->GetPose()); 

    // 如果上一帧为关键帧，或者单目的情况，则退出
    if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
        return;

    // Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点
    // 注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除

    // Create "visual odometry" MapPoints
    // We sort points according to their measured depth by the stereo/RGB-D sensor
    // Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）
    vector<pair<float,int> > vDepthIdx;
    vDepthIdx.reserve(mLastFrame.N);

    for(int i=0; i<mLastFrame.N;i++)
    {
        float z = mLastFrame.mvDepth[i];
        if(z>0)
        {
            // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
            vDepthIdx.push_back(make_pair(z,i));
        }
    }

    // 如果上一帧中没有有效深度的点,那么就直接退出
    if(vDepthIdx.empty())
        return;

    // 按照深度从小到大排序
    sort(vDepthIdx.begin(),vDepthIdx.end());

    // We insert all close points (depth<mThDepth)
    // If less than 100 close points, we insert the 100 closest ones.
    // Step 2.2：从中找出不是地图点的部分  
    int nPoints = 0;
    for(size_t j=0; j<vDepthIdx.size();j++)
    {
        int i = vDepthIdx[j].second;

        bool bCreateNew = false;

        // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点
        MapPoint* pMP = mLastFrame.mvpMapPoints[i];
        if(!pMP)
            bCreateNew = true;
        else if(pMP->Observations()<1)      
        {
            // 地图点被创建后就没有被观测，认为不靠谱，也需要重新创建
            bCreateNew = true;
        }

        if(bCreateNew)
        {
            // Step 2.3：需要创建的点，包装为地图点。只是为了提高双目和RGBD的跟踪成功率，并没有添加复杂属性，因为后面会扔掉
            // 反投影到世界坐标系中
            cv::Mat x3D = mLastFrame.UnprojectStereo(i);
            MapPoint* pNewMP = new MapPoint(
                x3D,            // 世界坐标系坐标
                mpMap,          // 跟踪的全局地图
                &mLastFrame,    // 存在这个特征点的帧(上一帧)
                i);             // 特征点id

            // 加入上一帧的地图点中
            mLastFrame.mvpMapPoints[i]=pNewMP; 

            // 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除
            mlpTemporalPoints.push_back(pNewMP);
            nPoints++;
        }
        else
        {
            // 因为从近到远排序，记录其中不需要创建地图点的个数
            nPoints++;
        }

        // Step 2.4：如果地图点质量不好，停止创建地图点
        // 停止新增临时地图点必须同时满足以下条件：
        // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
        // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
        if(vDepthIdx[j].first>mThDepth && nPoints>100)
            break;
    }
}

4.函数解释

4.1 利用参考关键帧更新上一帧在世界坐标系下的位姿

如上图，我们获得上一普通帧mLastFrame的参考关键帧pRef，获得ref_keyframe 到 lastframe的位姿变换Tlr，用Tlw = Tlr*Trw获得lastframe的位姿。

如果上一帧为关键帧，或者单目的情况，则退出。

4.2 对于双目或rgbd相机，为上一帧生成新的临时地图点

        注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除。

遍历上一帧LastFrame中具有有效深度值的特征点（不一定是地图点），记录特征点的深度和序号到vDepthIdx容器中。接着按照深度从小到大排序。

        遍历具有有效深度值的特征点vDepthIdx，用变量i记录是该特征点LastFrame中的哪个索引，如果这个点：

        ①对应在上一帧LastFrame中的地图点没有（!pMP）

②或者创建后就没有被观测到（pMP->Observations()<1）

        那么就生成一个临时的地图点

将这个特征点反投影到世界坐标系中得到3D点x3D，并包装成地图点pNewMP，加入上一帧LastFrame的地图点mvpMapPoints中，标记为临时添加的MapPointmlpTemporalPoints.，之后在CreateNewKeyFrame之前会全部删除。将标记nPoints++。

如果地图上有地图点我们也需要将nPoints++。

直到满足下列条件我们停止新增LastFrame的地图点：

①当前的点的深度已经超过了设定的深度阈值（35倍基线）（我们之前把地图点的深度从小到大排序）

②nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出