ORB-SLAM2源码学习：Tracking.cc：Tracking::TrackWithMotionModel恒速模型跟踪

前言

什么是恒速跟踪模型？

两个图像帧之间一般只有几十毫秒的时间，在这么短的时间内，可以做出如下合理的假设：

假设相机在短时间内的运动速度是恒定的，用之前帧的位姿和速度来预测当前帧的相机位姿，我们称之为恒速模型跟踪。

在系统初始化时，由于缺乏足够的信息（如速度信息），只能依赖于参考关键帧来进行粗略的位姿估计，这时常常会使用词袋匹配等简单的方法来获取初步位姿。这一步是“被逼无奈”的，因为此时没有足够的动态信息。然而，一旦参考关键帧跟踪成功并且速度信息得到了确定，系统就可以切换到更高效的恒速模型。恒速模型在此时非常合适，因为它假设在短时间内运动速度变化不大，可以直接利用前一帧的速度信息来估计当前位姿。这种方法比起参考关键帧跟踪要简单得多，同时也能显著提高跟踪的速度和效率，这对于实时性要求较高的SLAM系统是非常有意义的。

恒速模型跟踪的优缺点：跟踪仅需要上一帧的信息，是跟踪第一阶段中最常使用的跟踪方法。增加了一些技巧，可提高跟踪的稳定性。比如，在双目相机和RGB-D相机模式下生成的临时地图点可提高跟踪的成功率；投影匹配数目不足时没有马上放弃，而是将搜索范围扩大一倍再试一次。但恒速模型过于理想化，在帧率较低且运动变化较大的场景中可能会跟踪丢失。

1.函数声明

bool Tracking::TrackWithMotionModel()

2.函数定义

步骤：

1.更新上一帧的位姿，对于双目相机或者是RGBD相机来说，还会根据深度值生成临时地图点。

2.根据之前估计的速度，用恒速模型得到当前帧的初始位姿。

3.用上一帧的地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再试一次。

4. 利用3D-2D 投影关系，优化当前帧的位姿。

5.剔除地图点中的外点，并清除其所有关系。

6.统计匹配成功的特征点对数目，若超过阈值，则认为跟踪成功。

/*根据恒定速度模型用上一帧地图点来对当前帧进行跟踪1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点2：根据上一帧特征点对应地图点进行投影匹配3：优化当前帧位姿4：剔除地图点中外点return 如果匹配数大于10，认为跟踪成功，返回true*/
bool Tracking::TrackWithMotionModel()
{// 最小距离 < 0.9*次小距离 匹配成功，检查旋转ORBmatcher matcher(0.9,true);// Update last frame pose according to its reference keyframe// Create "visual odometry" points// Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点UpdateLastFrame();// Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿。mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);// 清空当前帧的地图点fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));// Project points seen in previous frame// 设置特征匹配过程中的搜索半径int th;if(mSensor!=System::STEREO)th=15;//单目elseth=7;//双目// Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);// If few matches, uses a wider window search// 如果匹配点太少，则扩大搜索半径再来一次if(nmatches<20){fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th}// 如果还是不能够获得足够的匹配点,那么就认为跟踪失败if(nmatches<20)return false;// Optimize frame pose with all matches// Step 4：利用3D-2D投影关系，优化当前帧位姿Optimizer::PoseOptimization(&mCurrentFrame);// Discard outliers// Step 5：剔除地图点中外点int nmatchesMap = 0;for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){if(mCurrentFrame.mvbOutlier[i]){// 如果优化后判断某个地图点是外点，清除它的所有关系MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)// 累加成功匹配到的地图点数目nmatchesMap++;}}    if(mbOnlyTracking){// 纯定位模式下：如果成功追踪的地图点非常少,那么这里的mbVO标志就会置位mbVO = nmatchesMap<10;return nmatches>20;}// Step 6：匹配超过10个点就认为跟踪成功return nmatchesMap>=10;
}