详谈ORB-SLAM2的跟踪线程Tracking

ORB-SLAM2的三大线程几乎都是每个部分对应一个函数，非常清晰。函数名起的都十分贴切，望文就能生义，我们更应该关注的是里面的关键帧地图点这些变量是怎么流转的。

Tracking线程的流程就是：首先输入一帧图像，然后实际上接着进行5步。初始化（实际上相机刚开始需要初始化过程）、图像预处理（提取ORB特征点）、初始位姿估计（根据前一张图片或者附近的一张图片估计当前帧的位姿）、基于当前帧的位姿构建当前帧的局部地图（当前帧附近的很小的一部分地图）、是否生成新的关键帧

PS：通过追踪局部帧的地图而建图，实际上后面有专门负责建设局部地图的线程，所以这里的局部地图并没有用来建图，只是用来优化当前帧的位姿

一、跟踪线程各成员函数/变量

1、跟踪状态

(1)枚举类型eTrackingState

Tracking类中定义枚举类型eTrackingState，表示跟踪状态，系统创建时设值SYSTEM_NOT_READY，加载好后设值NO_IMAGES_YET，对于单目视觉至少有两帧图像才能成功，对于双目视觉需要一帧图像就鞥初始化成功，如果一帧图像的特征点过少也不能初始化成功

值	意义
SYSTEM_NOT_READY	系统没有准备好,一般就是在启动后加载配置文件和词典文件时候的状态
NO_IMAGES_YET	还没有接收到输入图像
NOT_INITIALIZED	接收到图像但未初始化成功
OK	跟踪成功
LOST	跟踪失败

(2)Tracking类的成员变量

Tracking类的成员变量mState和mLastProcessedState分别表示当前帧的跟踪状态和上一帧的跟踪状态

成员变量	访问控制	意义
eTrackingState mState	public	当前帧mCurrentFrame的跟踪状态
eTrackingState mLastProcessedState	public	前一帧mLastFrame的跟踪状态

(3)跟踪状态流程

此博客参考ncepu_Chen的《5小时让你假装大概看懂ORB-SLAM2源码》

void Tracking::Track() {// ...unique_lock<mutex> lock(mpMap->mMutexMapUpdate);// step1. 若还没初始化,则尝试初始化if (mState == NOT_INITIALIZED) {if (mSensor == System::STEREO || mSensor == System::RGBD)StereoInitialization();elseMonocularInitialization();if (mState != OK)return;} // ...
}

void Tracking::CreateInitialMapMonocular() {// mInitialFrame 和 mCurrentFrame 是最早的两个关键帧KeyFrame *pKFini = new KeyFrame(mInitialFrame, mpMap, mpKeyFrameDB);  KeyFrame *pKFcur = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);  // step1. 计算两个关键帧的词袋pKFini->ComputeBoW();pKFcur->ComputeBoW();// step2. 将两个关键帧插入地图mpMap->AddKeyFrame(pKFini);mpMap->AddKeyFrame(pKFcur);// step3. 处理所有地图点for (size_t i = 0; i < mvIniMatches.size(); i++) {// 创建并添加地图点MapPoint *pMP = new MapPoint(mvIniP3D[i], pKFcur, mpMap);		mpMap->AddMapPoint(pMP);// 添加关键帧到地图点的观测pKFini->AddMapPoint(pMP, i);pKFcur->AddMapPoint(pMP, mvIniMatches[i]);// 添加地图点到关键帧的观测pMP->AddObservation(pKFini, i);pMP->AddObservation(pKFcur, mvIniMatches[i]);// 计算地图点描述子并更新平均观测数据pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();}// 基于观测到的地图点,更新关键帧共视图pKFini->UpdateConnections();pKFcur->UpdateConnections();// step4. 全局BA: 优化所有关键帧位姿和地图点Optimizer::GlobalBundleAdjustemnt(mpMap, 20);// step5. 将两帧间的平移尺度归一化(以场景的中值深度为参考)float medianDepth = pKFini->ComputeSceneMedianDepth(2);float invMedianDepth = 1.0f / medianDepth;cv::Mat Tc2w = pKFcur->GetPose();Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth;pKFcur->SetPose(Tc2w);// step6. 将坐标点尺度也归一化vector<MapPoint *> vpAllMapPoints = pKFini->GetMapPointMatches();for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++) {if (vpAllMapPoints[iMP]) {MapPoint *pMP = vpAllMapPoints[iMP];pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);}}// step7. 将关键帧插入局部地图,更新归一化后的位姿和地图点坐标  mpLocalMapper->InsertKeyFrame(pKFini);mpLocalMapper->InsertKeyFrame(pKFcur);mCurrentFrame.SetPose(pKFcur->GetPose());mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKFcur;mvpLocalKeyFrames.push_back(pKFcur);mvpLocalKeyFrames.push_back(pKFini);mvpLocalMapPoints = mpMap->GetAllMapPoints();mpReferenceKF = pKFcur;mCurrentFrame.mpReferenceKF = pKFcur;mLastFrame = Frame(mCurrentFrame);mpMap->SetReferenceMapPoints(mvpLocalMapPoints);mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());mpMap->mvpKeyFrameOrigins.push_back(pKFini);// step8. 更新跟踪状态变量mStatemState = OK;
}

void Tracking::StereoInitialization() {if (mCurrentFrame.N > 500) {// 基于当前帧构造初始关键帧mCurrentFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));KeyFrame *pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);mpMap->AddKeyFrame(pKFini);mpLocalMapper->InsertKeyFrame(pKFini);// 构造地图点for (int i = 0; i < mCurrentFrame.N; i++) {float z = mCurrentFrame.mvDepth[i];if (z > 0) {cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);MapPoint *pNewMP = new MapPoint(x3D, pKFini, mpMap);pNewMP->AddObservation(pKFini, i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);pKFini->AddMapPoint(pNewMP, i);mCurrentFrame.mvpMapPoints[i] = pNewMP;}}// 构造局部地图mvpLocalKeyFrames.push_back(pKFini);mvpLocalMapPoints = mpMap->GetAllMapPoints();mpReferenceKF = pKFini;mCurrentFrame.mpReferenceKF = pKFini;// 更新跟踪状态变量mStatemState = OK;}
}