一、函数作用

本函数的作用是追踪帧，时追踪线程的主体，也是最重要的部分

二、完整的源码及注释

void Tracking::Track()
{// 判断是否需要初始化// mState为tracking的状态，包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态if(mState==NO_IMAGES_YET){mState = NOT_INITIALIZED;}//  储存mState的状态mLastProcessedState=mState;// 地图更新时加锁。保证地图不会发生变化// 疑问:这样子会不会影响地图的实时更新?// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图 unique_lock<mutex> lock(mpMap->mMutexMapUpdate);// 如果地图没有初始化，就进行初始化if(mState==NOT_INITIALIZED){// 如果是双目或者RGBD则进行双目初始化if(mSensor==System::STEREO || mSensor==System::RGBD)StereoInitialization();// 否则单目初始化elseMonocularInitialization();// 初始化函数的结尾，都将mState状态置为OK// 更新帧绘制器中存储的最新状态mpFrameDrawer->Update(this);if(mState!=OK)return;}// 如果已经初始化了else{// bOK为临时变量，用于表示每个函数是否执行成功bool bOK;// 判断是否为SLAM模式，mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式if(!mbOnlyTracking){// 判断跟踪状况是否正常if(mState==OK){// Local Mapping might have changed some MapPoints tracked in last frame// 检查上一帧的地图点是否被替换，若被替换则更新地图点为替换的点CheckReplacedInLastFrame();// 运动模型是空的或刚完成重定位if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2){// 满足上述条件，使用参考关键帧的方法跟踪bOK = TrackReferenceKeyFrame();}// 不满足则用恒速模型跟踪else{// 恒速模型跟踪bOK = TrackWithMotionModel();if(!bOK)// 如果恒速模型跟踪返回值为false（跟踪失败），则用参考关键帧的方法跟踪bOK = TrackReferenceKeyFrame();}}// 跟踪状态不正常则重新定位else{bOK = Relocalization();}}// 如果为仅定位模式判断是否跟丢else{// 判断是否跟丢if(mState==LOST){// 如果跟丢，则重新定位bOK = Relocalization();}// 没跟丢的话，判断是否有足够的特征点else{// 判断此帧匹配的特征点数量是否充足if(!mbVO)  // mbVO = false表示此帧匹配了很多的特征点，mbVO = true则表示该帧匹配的特征点很少{// 充足则判断速度是否为空if(!mVelocity.empty()){// 速度不空则用横速模型跟踪bOK = TrackWithMotionModel();}// 否则用参考关键点的方法跟踪else{bOK = TrackReferenceKeyFrame();}}// 特征点数量不足，则恒速模型和重新定位一起使用else{// In last frame we tracked mainly "visual odometry" points.// We compute two camera poses, one from motion model and one doing relocalization.// If relocalization is sucessfull we choose that solution, otherwise we retain// the "visual odometry" solution.bool bOKMM = false;bool bOKReloc = false;vector<MapPoint*> vpMPsMM;vector<bool> vbOutMM;cv::Mat TcwMM;if(!mVelocity.empty()){bOKMM = TrackodWithMotionMel();vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}bOKReloc = Relocalization();if(bOKMM && !bOKReloc){mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;if(mbVO){for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){mCurrentFrame.mvpMapPoints[i]->IncreaseFound();}}}}else if(bOKReloc){mbVO = false;}bOK = bOKReloc || bOKMM;}}}mCurrentFrame.mpReferenceKF = mpReferenceKF;// 判断是否为SLAM模式if(!mbOnlyTracking){// if(bOK)  // 这里的bOK = bOKReloc || bOKMM，即恒速模型跟踪和定位跟踪是否有一个成功完成// 有一个完成就局部跟踪建图bOK = TrackLocalMap();}// 如果是仅定位模式else{// 如果跟踪成功，则局部建图跟踪if(bOK && !mbVO)bOK = TrackLocalMap();}// 如果跟踪成功，则更新状态为OKif(bOK)mState = OK;// 否则跟踪丢失elsemState=LOST;// 更新显示线程中的图像、特征点、地图点等信息mpFrameDrawer->Update(this);// If tracking were good, check if we insert a keyframe// 只有在成功追踪时才考虑生成关键帧的问题if(bOK){// Update motion model、// 跟踪成功，更新恒速运动模型if(!mLastFrame.mTcw.empty()){cv::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 = mCurrentFrame.mTcw*LastTwc;}else// 否则速度为空mVelocity = cv::Mat();// 更新显示中的位姿mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);// Clean VO matches// 清除观测不到的地图点 for(int i=0; i<mCurrentFrame.N; i++){MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(pMP)if(pMP->Observations()<1){mCurrentFrame.mvbOutlier[i] = false;mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Delete temporal MapPoints// 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;// 这里是释放指针，避免内存泄露delete pMP;}// 清除临时添加的地图点mlpTemporalPoints.clear();// Check if we need to insert a new keyframe// 检测并插入关键帧，对于双目或RGB-D会产生新的地图点if(NeedNewKeyFrame())CreateNewKeyFrame();// We allow points with high innovation (considererd outliers by the Huber Function)// pass to the new keyframe, so that bundle adjustment will finally decide// if they are outliers or not. We don't want next frame to estimate its position// with those points so we discard them in the frame.// 删除那些在bundle adjustment中检测为outlier的地图点for(int i=0; i<mCurrentFrame.N;i++){if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Reset if the camera get lost soon after initialization// 如果初始化后不久就跟踪失败，并且重新定位后也没有搞定，只能重置if(mState==LOST){if(mpMap->KeyFramesInMap()<=5){cout << "Track lost soon after initialisation, reseting..." << endl;mpSystem->Reset();return;}}// 确保已经设置了参考关键帧if(!mCurrentFrame.mpReferenceKF)mCurrentFrame.mpReferenceKF = mpReferenceKF;mLastFrame = Frame(mCurrentFrame);}// Store frame pose information to retrieve the complete camera trajectory afterwards.// 记录位姿信息，用于最后保存所有的轨迹if(!mCurrentFrame.mTcw.empty()) // 如果当前帧的相机位姿不空{cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();mlRelativeFramePoses.push_back(Tcr);mlpReferences.push_back(mpReferenceKF);mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);mlbLost.push_back(mState==LOST);}// 如果跟踪失败，则用上一次的值else{// This can happen if tracking is lostmlRelativeFramePoses.push_back(mlRelativeFramePoses.back());mlpReferences.push_back(mlpReferences.back());mlFrameTimes.push_back(mlFrameTimes.back());mlbLost.push_back(mState==LOST);}}

三、函数讲解

本函数代码量太大，将分段讲解，函数中调用的函数超过20个，将在后续的文章中一一讲解，这篇文章讲的是这个函数的框架和主体

1. 判断是否初始化，未初始化则初始化

1）讲解

这段代码的作用是，判断是是否初始化如果『图像复位过、或者第一次运行』，则mState为NO_IMAGE_YET状态，就需要初始化

2）源码

if(mState==NO_IMAGES_YET){mState = NOT_INITIALIZED;}//  储存mState的状态mLastProcessedState=mState;// 地图更新时加锁。保证地图不会发生变化// 疑问: 这样子会不会影响地图的实时更新?// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图 unique_lock<mutex> lock(mpMap->mMutexMapUpdate);// 如果地图没有初始化，就进行初始化if(mState==NOT_INITIALIZED){// 如果是双目或者RGBD则进行双目初始化if(mSensor==System::STEREO || mSensor==System::RGBD)StereoInitialization();// 否则单目初始化elseMonocularInitialization();// 初始化函数的结尾，都将mState状态置为OK// 更新帧绘制器中存储的最新状态mpFrameDrawer->Update(this);if(mState!=OK)return;}

2. 根据具体条件判断跟踪方式（三大跟踪方式）

1）讲解

本段代码的作用是，根据不同的情况选择跟踪方式，这段代码的逻辑非常复杂，但是一边看逻辑图（下图），一边看代码就会轻松很多，读者要做到的是，将每个if和逻辑图中的步骤对应起来。

本函数中的三种跟踪方式为：

1. 参考关键帧跟踪：TrackReferenceKeyFrame();
2. 恒速模型跟踪：TrackWithMotionModel();
3. 重定位跟踪：Relocalization();
4. 局部地图跟踪：TrackLocalMap();

2）源码

// 判断是否为SLAM模式，mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式if(!mbOnlyTracking){// 判断跟踪状况是否正常if(mState==OK){// Local Mapping might have changed some MapPoints tracked in last frame// 检查上一帧的地图点是否被替换，若被替换则更新地图点为替换的点CheckReplacedInLastFrame();// 运动模型是空的或刚完成重定位if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2){// 满足上述条件，使用参考关键帧的方法跟踪bOK = TrackReferenceKeyFrame();}// 不满足则用恒速模型跟踪else{// 恒速模型跟踪bOK = TrackWithMotionModel();if(!bOK)// 如果恒速模型跟踪返回值为false（跟踪失败），则用参考关键帧的方法跟踪bOK = TrackReferenceKeyFrame();}}// 跟踪状态不正常则重新定位else{bOK = Relocalization();}}// 如果为仅定位模式判断是否跟丢else{// 判断是否跟丢if(mState==LOST){// 如果跟丢，则重新定位bOK = Relocalization();}// 没跟丢的话，判断是否有足够的特征点else{// 判断此帧匹配的特征点数量是否充足if(!mbVO)  // mbVO = false表示此帧匹配了很多的特征点，mbVO = true则表示该帧匹配的特征点很少{// 充足则判断速度是否为空if(!mVelocity.empty()){// 速度不空则用横速模型跟踪bOK = TrackWithMotionModel();}// 否则用参考关键点的方法跟踪else{bOK = TrackReferenceKeyFrame();}}// 特征点数量不足，则恒速模型和重新定位一起使用else{// In last frame we tracked mainly "visual odometry" points.// We compute two camera poses, one from motion model and one doing relocalization.// If relocalization is sucessfull we choose that solution, otherwise we retain// the "visual odometry" solution.bool bOKMM = false;bool bOKReloc = false;vector<MapPoint*> vpMPsMM;vector<bool> vbOutMM;cv::Mat TcwMM;if(!mVelocity.empty()){bOKMM = TrackodWithMotionMel();vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}bOKReloc = Relocalization();if(bOKMM && !bOKReloc){mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;if(mbVO){for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){mCurrentFrame.mvpMapPoints[i]->IncreaseFound();}}}}else if(bOKReloc){mbVO = false;}bOK = bOKReloc || bOKMM;}}}

3. 如果三个种追踪方式中有一个追踪成功，则进行局部地图追踪

1）讲解

这代码是上图的下方部分，因为文件中（ORB-SLAM2）调用此函数有两种定位模式，一种SLAM模式（定位+跟踪+建图），一种仅定位模式（定位+跟踪），所以判断是否进行局部建图跟踪的条件不一样，如果更新成功，则更新状态为OK，否则更新状态为LOST

2）源码

// 判断是否为SLAM模式if(!mbOnlyTracking){// if(bOK)  // 这里的bOK = bOKReloc || bOKMM，即恒速模型跟踪和重定位跟踪是否有一个成功完成// 有一个完成就局部跟踪建图bOK = TrackLocalMap();}// 如果是仅定位模式else{// 如果跟踪成功，则局部建图跟踪if(bOK && !mbVO)bOK = TrackLocalMap();}// 如果跟踪成功，则更新状态为OKif(bOK)mState = OK;// 否则跟踪丢失elsemState=LOST;

4. 跟踪成功后的后续操作

1）讲解

本段代码为跟踪成功后的后续操作，主要作用是：

1. 跟踪成功，更新恒速运动模型
2. 清除观测不到的地图点
3. 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
4. 检测并插入关键帧，对于双目或RGB-D会产生新的地图点
5. 删除那些在bundle adjustment中检测为outlier的地图点

2）源码

				// 只有在成功追踪时才考虑生成关键帧的问题if(bOK){// Update motion model、// 跟踪成功，更新恒速运动模型if(!mLastFrame.mTcw.empty()){cv::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 = mCurrentFrame.mTcw*LastTwc;}else// 否则速度为空mVelocity = cv::Mat();// 更新显示中的位姿mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);// Clean VO matches// 清除观测不到的地图点 for(int i=0; i<mCurrentFrame.N; i++){MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(pMP)if(pMP->Observations()<1){mCurrentFrame.mvbOutlier[i] = false;mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Delete temporal MapPoints// 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;// 这里是释放指针，避免内存泄露delete pMP;}// 清除临时添加的地图点mlpTemporalPoints.clear();// Check if we need to insert a new keyframe// 检测并插入关键帧，对于双目或RGB-D会产生新的地图点if(NeedNewKeyFrame())CreateNewKeyFrame();// We allow points with high innovation (considererd outliers by the Huber Function)// pass to the new keyframe, so that bundle adjustment will finally decide// if they are outliers or not. We don't want next frame to estimate its position// with those points so we discard them in the frame.// 删除那些在bundle adjustment中检测为outlier的地图点for(int i=0; i<mCurrentFrame.N;i++){if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}

5. 重定位后仍然跟踪失败，则重置

1）讲解

如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset

2）源码

				if(mState==LOST){//如果地图中的关键帧信息过少的话,直接重新进行初始化了if(mpMap->KeyFramesInMap()<=5){cout << "Track lost soon after initialisation, reseting..." << endl;mpSystem->Reset();return;}}

6. 保存此次跟踪的成果

1）讲解

记录位姿信息，用于最后保存所有的轨迹

2）源码

if(!mCurrentFrame.mTcw.empty()){// 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();//保存各种状态mlRelativeFramePoses.push_back(Tcr);mlpReferences.push_back(mpReferenceKF);mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);mlbLost.push_back(mState==LOST);}else{// This can happen if tracking is lost// 如果跟踪失败，则相对位姿使用上一次值mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());mlpReferences.push_back(mlpReferences.back());mlFrameTimes.push_back(mlFrameTimes.back());mlbLost.push_back(mState==LOST);}

四、总结

此函数为跟踪线程的最主要部分，在不同条件下调用不同的跟踪方式，然后进行局部建图跟踪，最后保存这此跟踪的成果（相机的位姿等），而且本函数除了明线外，还参与了贯穿三条主线程的一条暗线，因为读者后读到这里还未学完三个主线程，这里就给读者留下一个悬念，在后续文章中会讲到，如果有需要也欢迎大家和本人交流。