详谈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();
        else
            MonocularInitialization();
		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. 更新跟踪状态变量mState
    mState = 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;
		
        // 更新跟踪状态变量mState
        mState = OK;
    }
}