目录

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第一个元素是某个点的深度,第二个元素是对应的特征点idvDepthIdx.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 利用参考关键帧更新上一帧在世界坐标系下的位姿

         如上图，我们获得上一普通帧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个点，说明距离比较远了，可能不准确，停掉退出