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

目录

1.函数作用

2.步骤 

3.code 

4.函数解释

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 > vDepthIdx;
    vDepthIdx.reserve(mLastFrame.N);

    for(int i=0; i0)
        {
            // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
            vDepthIdx.push_back(make_pair(z,i));
        }
    }

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

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

    // We insert all close points (depthObservations()<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 利用参考关键帧更新上一帧在世界坐标系下的位姿

ORB-SLAM2 ---- Tracking::UpdateLastFrame函数_第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个点,说明距离比较远了,可能不准确,停掉退出

你可能感兴趣的:(orb-slam2,算法,c++,计算机视觉,slam)