目录

1.函数作用

2.步骤 

3.code 

4.函数解释 

4.1 更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点

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

4.3 用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次 

4.4 利用3D-2D投影关系，优化当前帧位姿 

4.5 剔除地图点中外点

1.函数作用

        用最近的普通帧来跟踪当前的普通帧。根据恒速模型设定当前帧的初始位姿，通过投影的方式在参考帧中找当前帧特征点的匹配点，优化每个特征点所对应3D点的投影误差即可得到位姿。

2.步骤 

 * @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
 * Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
 * Step 2：根据上一帧特征点对应地图点进行投影匹配
 * Step 3：优化当前帧位姿
 * Step 4：剔除地图点中外点
 * @return 如果匹配数大于10，认为跟踪成功，返回true

3.code 

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(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(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(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;
}

4.函数解释 

4.1 更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点

ORB-SLAM2 ---- Tracking::UpdateLastFrame函数解析https://blog.csdn.net/qq_41694024/article/details/128192577        通过这一步，我们计算了LastFrame的位姿并且对于双目或RGB-D相机来说，我们新增了一些地图点。

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

         当前帧本身是没有位姿的，恒速模型设置当前帧的位姿为速度mVelocity * 上一帧的位姿得到当前帧的初始位姿。

        我们看速度mVelocity 的更新：

                // 更新恒速运动模型 TrackWithMotionModel 中的mVelocitycv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));// mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwcmVelocity = mCurrentFrame.mTcw*LastTwc; }

        它其实是上一帧到当前帧的变换，它不断在更新，因此“上一帧到当前帧的变换”mVelocity （lastFrame的上一帧到lastFrame的变换） * 上一帧的位姿（lastFrame的位姿），即我们假设相同两帧的变化幅度不大，用LastFrame的LastFrame到LastFrame的变换来代替LastFrame到CurrentFrame的变化矩阵，但这仅仅是个初始值。

        随后清空当前帧CurrentFrame的地图点，根据相机类型选择匹配半径。

4.3 用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次 

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

        如果匹配点太少，则扩大搜索半径再来一次。

        如果还是不能够获得足够的匹配点，那么就认为跟踪失败。

4.4 利用3D-2D投影关系，优化当前帧位姿 

4.5 剔除地图点中外点

        如果我们经过优化后确定当前地图点是外点，则清除它的所有关系：

        ①将当前帧的地地图点mvpMapPoints[i]的内存清空

        ②设置当前地图点的外点的标记mvbOutlier为NULL。属于外点的特征点标记，在 Optimizer::PoseOptimization 使用了。

        ③将标记mbTrackInView设置为false

mbTrackInView==false的点有几种：
 a 已经和当前帧经过匹配（TrackReferenceKeyFrame，TrackWithMotionModel）但在优化过程中认为是外点
b 已经和当前帧经过匹配且为内点，这类点也不需要再进行投影 
c 不在当前相机视野中的点（即未通过isInFrustum判断）

        ④将标记mnLastFrameSeen设置为false

mnLastFrameSeen==mCurrentFrame.mnId的点有几种：
a 已经和当前帧经过匹配（TrackReferenceKeyFrame，TrackWithMotionModel）但在优化过程中认为是外点
b 已经和当前帧经过匹配且为内点，这类点也不需要再进行投影