ORB-SLAM2源码解读（2.1）：Tracking

Tracking是SLAM的灵魂，更像是前端里程计VO，这里Tracking的主要任务两方面：（1）完成相机位姿估计（2）跟踪局部地图

思路：TrackLocalMap（）在当前帧和局部地图之间找到尽可能多的对应关系，优化当前帧的位姿。对每一帧都进行跟踪

第一次接触这么大的工程，发现之前接触的真的好弱鸡，总结下来是：先看基本流程图+从头到尾啃一遍代码+总结

这里要感谢吴博和吃水的鱼和蚁族的坚持的博客，大神的博客给了很大的启发。

下面重点研究单目作传感器

Tracking线程代码主要有两部分:Track()函数为主线，各部分成员函数为分支。第一部分主要讲Track（）函数部分代码，下一讲详细介绍各分支代码及原理。

---

基本流程框架：

1、单目初始化 MonocularInitialization()

2、相机位姿跟踪：（1）跟踪运动模型TrackWithMotionModel()（2）跟踪关键帧TrackReferenceKeyFrame()（3）重定位Relocalization()

3、跟踪局部地图 TrackLocalMap()

流程图1：

流程图2

 

---

Tracking线程主要函数代码逻辑

MonocularInitialization()

最初的两帧进行对极约束和全局BA优化，计算两帧位姿、三角化得到地图点深度

1、创建单目初始器mpInitializer，用Frame()函数构造第一帧mInitialFrame
2、如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
3、在mInitialFrame与mCurrentFrame中找匹配的特征点对
4、如果初始化的两帧之间的匹配点太少，重新初始化
5、通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
6、CreateInitialMapMonocular()将三角化得到的3D点包装成MapPoints

TrackWithMotionModel()

假设匀速运动，用上一帧位姿和速度估计当前帧位姿。方法：上一帧地图点投影到当前帧，完成匹配。

1、假设：两帧间相对运动不变，根据上一帧位姿和速度用PnP估计当前帧位姿。
2、SearchByProjection（）在将上一帧的地图点投影到当前固定大小范围的帧平面上，如果匹配点少，那么扩大两倍的采点范围
3、PoseOptimization（）优化相机位姿，BA算法，最小二乘法。
4、优化位姿后剔除outlier的mvpMapPoints

TrackReferenceKeyFrame()

关键帧中查找BOW相近的帧，进行匹配优化位姿。方法：关键帧BOW向量与当前帧进行匹配

1、mCurrentFrame.ComputeBoW()将当前帧的描述子转为BOW向量，加块匹配速度
2、matcher.SearchByBoW()通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
3、mCurrentFrame.SetPose(mLastFrame.mTcw)将上一帧的位姿态作为当前帧位姿的初始值
4、PoseOptimization通过优化3D-2D的重投影误差来获得位姿
5、剔除优化后的outlier匹配点（MapPoints）

Relocalization()

重定位，之前所有关键帧中查找与当前帧有充足匹配点的候选帧，Ransac迭代，PnP求解位姿

1、计算当前帧特征点的Bow映射
2、找到与当前帧相似的候选关键帧，mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame)
3、通过EPnP算法估计姿态
4、通过PoseOptimization对姿态进行优化求解
5、如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解

TrackLocalMap()

投影，从已经生成的地图点中找到更多对应关系。

1、UpdateLocalMap()更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
2、SearchLocalPoints()在局部地图中查找与当前帧匹配的MapPoints
3、PoseOptimization()更新局部所有MapPoints后对位姿再次优化
4、更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果

NeedNewKeyFrame

判断是否需要生成新的关键帧，确定关键帧的标准

1. 在上一次进行重定位之后，过了20帧数据，或关键帧数小于20个，不满足不能生成
2. 在上一个关键帧插入之后，过了20帧，或 局部建图是空闲状态，不满足不能生成。
3. 当前帧跟踪到大于若干个点，不满足不能生成
4. 当前帧的跟踪点数小于90%的参考关键帧跟踪点数，并且当前帧跟踪点数大于15，不满足不能生成

 

---

具体流程：

一、Tracking的两个模式

1、仅追踪模式(mbOnlyTracking)：局部地图不工作，只追踪地图现有地图点

2、同时定位与建图( !mbOnlyTracking )：追踪线程同时有局部建图和回环检测。

二、Tracking的四种状态

NO_IMAGES_YET表示当前没有图片，NOT_INITIALIZED表示当前没有初始化追踪线程，OK证明当前追踪线程完好，LOST证明当前追踪线程丢失——注意这里的线程状态都是指当前帧处理之前的状态。

处于NO_IMAGES_YET，当新的一帧来临时，将线程状态改变为NOT_INITIALIZED。

处于NOT_INITIALIZED，则针对单目相机和双目相机/RGBD相机进行不同的初始化，步骤 2

处于OK，经过初始化的系统追踪线程转为OK状态，首先CheckReplacedInLastFrame()检测上一帧中的所有地图点中有没有可以代替该地图点的其他地图点，如果有则将两地图点融合为一个地图点（用替代点替代当前帧中的地图点）。然后用两种方式求解相机位姿 ，步骤 3 4 5  7

处于LOST，重定位，步骤6

1、在进行追踪线程之前，首先对输入的图像进行预处理，GrabImageMonocular（）先将RGB图像化为灰度图像，之后Frame() 建立新帧，提取特征点。数据流以Frame的形式进入Track()函数。

2、单目初始化：需要两帧，第一帧建立初始化器，设定该帧作为初始化参考帧；第二帧作为匹配帧，进行特征点匹配，进而通过并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态，三角化得到地图点的深度信息。注意：单目初始化得到的深度信息同样具有不确定性，需要将得到的地图点深度归一化，位移向量也归一化，之后的追踪线程计算的地图点和相机位姿都是相对于初始化时的距离展开的。

MonocularInitialization();//单目初始化

3、跟踪运动模型：假设：匀速运动模型，两帧相对运动相同。这里匹配是通过投影来与上一帧看到的地图点匹配，使用的是matcher.SearchByProjection()，并根据上一帧的位姿和速度预测当前相机的位姿，同样将匹配地图点和位姿加入到图优化中得到当前帧的位姿以及匹配内点

bOK = TrackWithMotionModel();

4、跟踪参考关键帧（上一帧速度为0或当前帧与上一次重定位帧之间ID差大于2）：根据参考关键帧与当前帧匹配得到匹配地图点，将上一参考帧位姿作为当前帧的初始位姿，然后将匹配点和位姿同时进行图优化，得到当前帧的位姿以及匹配内点

if(mVelocity.empty() || mCurrentFrame.mnId

5、重定位：

（1）计算当前帧的BoW映射

（2）找到与当前帧相似的候选关键帧

（3）匹配当前帧与找到的候选关键帧，计算相机位姿Tcw（RANSAC+PNP算法）

（4）优化相机位姿Tcw

bOK = Relocalization();

6、跟踪局部地图：对相机位姿有个基本估计和初始特征匹配，将局部地图投影到当前帧寻找更多对应的匹配地图点MapPoints，优化相机位姿。

（1）UpdateLocalMap（）更新局部地图（关键帧+局部地图点）；

（2）SearchLocalPoints（）匹配局部地图视野范围内的点和当前帧特征点；

（3）PoseOptimization（）根据新的匹配点重新优化相机位姿。

bOK = TrackLocalMap();//跟踪局部地图

7、跟踪成功，需要

（1）根据优化的相机位姿更新恒速运动模型TrackWithMotionModel（）

（2）并清除临时地图点（UpdateLastFrame()函数针对双目和RGB-D，需要根据深度值对上一关键帧产生临时的MapPoints），删除地图点中是外点的地图点

（3）更新参考关键帧（参考关键帧是所有关键帧中公式地图点最多的关键帧）。检测此时是否需要添加关键帧NeedNewKeyFrame()，这里添加关键帧是给局部地图线程添加关键帧，然后在局部地图中处理完成后将关键帧传递给回环检测。

（4）跟踪成功记录当前位姿信息，跟踪失败则使用上一次位姿值。

---

void  Tracking：：Track()

    // track包含两部分：估计运动、跟踪局部地图
    // mState为tracking的状态机
    // 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
    if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }
    // mLastProcessedState存储了Tracking最新的状态，用于FrameDrawer中的绘制
    mLastProcessedState=mState;

    // 上锁Get Map Mutex -> Map cannot be changed
    unique_lock lock(mpMap->mMutexMapUpdate);

处于NO_IMAGES_YET， 有关unique_lock是为了上锁，保证多线程不能同时修改共享数据，这里对地图上锁。

    // 步骤1：初始化
    if(mState==NOT_INITIALIZED)
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();//单目初始化

        mpFrameDrawer->Update(this);

        if(mState!=OK)
            return;//只是return 相当于break
    }
    else// 步骤2：正常跟踪OK
    {
        // System is initialized. Track Frame.
		// 系统已初始化，开始跟踪帧
        // bOK为临时变量，用于表示每个函数是否执行成功
        bool bOK;

        // 初始相机位姿估计用运动模型或重定位(跟踪丢了)
        // 在viewer中有个开关menuLocalizationMode，有它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
        // mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
        if(!mbOnlyTracking)//激活了局部地图
        {
            // 正常初始化成功
            if(mState==OK)
            {
                // 检查并更新上一帧被替换的MapPoints
                // 更新Fuse函数和SearchAndFuse函数替换的MapPoints
                CheckReplacedInLastFrame();

                // 步骤2.1：跟踪上一帧或者参考帧或者重定位

                // 上一帧速度为0或当前帧与上一次重定位帧之间ID差大于2，跟踪关键帧
                // mnLastRelocFrameId上一次重定位的那一帧
                if(mVelocity.empty() || mCurrentFrame.mnId vpMPsMM;
                    vector vbOutMM;
                    cv::Mat TcwMM;

                    if(!mVelocity.empty())//跟踪模型
                    {
                        bOKMM = TrackWithMotionModel();
                        // 这三行没啥用？
                        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)
                        {
                            // 这段代码是不是有点多余？应该放到TrackLocalMap函数中统一做
                            // 更新当前帧的MapPoints被观测程度 
                            for(int i =0; iIncreaseFound();
                                }
                            }
                        }
                    }
                    else if(bOKReloc)// 只要重定位成功整个跟踪过程正常进行（定位与跟踪，更相信重定位）
                    {
                        mbVO = false;
                    }

                    bOK = bOKReloc || bOKMM;
                }
            }
        }

        // 将最新的关键帧作为reference frame
        mCurrentFrame.mpReferenceKF = mpReferenceKF;

处于NOT_INITIALIZED，进行单目初始化即可。

处于OK，按照两种相机位姿追踪模式得到初始位姿估计，

1、局部地图激活(!mbOnlyTracking)：如果(mState==OK)，CheckReplacedInLastFrame()首先更新上一帧被替换的MapPoints，然后如果特征点匹配太少，需要匹配参考关键帧bOK = TrackReferenceKeyFrame()，否则根据匀速运动模型匹配bOK = TrackWithMotionModel()。特殊情况初始化跟踪失败需要重定位bOK = Relocalization(); 

2、局部地图不工作，只跟踪当前地图中地图点：如果(mState==LOST)，需要重定位Relocalization()。

（1）正常情况下需要先看匹配的特征点数量是否足够多 (!mbVO），接下来上一帧有速度（!mVelocity.empty()）匀速运动模型TrackWithMotionModel()，上一帧没速度跟踪参考关键帧TrackReferenceKeyFrame()。

（2）当匹配特征点不够多，小于10个，使用运动模型和重定位计算两种相机位姿，如果重定位失败，保持VO结果，否则更相信重定位结果。

        if(!mbOnlyTracking)//不是只跟踪还插入关键帧，局部地图工作
        {
            if(bOK)
                bOK = TrackLocalMap();//跟踪局部地图
        }
        else
        {
            // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
            // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
            // the camera we will use the local map again.

            // 局部地图不工作，特征点足够且重定位成功
            if(bOK && !mbVO)
                bOK = TrackLocalMap();
        }

        if(bOK)
            mState = OK;
        else
            mState=LOST;

        // Update drawer
        mpFrameDrawer->Update(this);

        // If tracking were good, check if we insert a keyframe
		// 跟踪成功，检查是否插入了关键帧
        if(bOK)
        {
            // 先更新运动模型
            if(!mLastFrame.mTcw.empty())
            {
                // 步骤2.3：更新恒速运动模型TrackWithMotionModel中的mVelocity
                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; // Tcl
            }
            else
                mVelocity = cv::Mat();

            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

            // 清除 VO matches
            // 步骤2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
            for(int i=0; iObservations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
                    }
            }

            // 清除临时地图点
            // 步骤2.5：清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
            // 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
            // 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
            for(list::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }
            // 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
            mlpTemporalPoints.clear();

            // Check if we need to insert a new keyframe
            // 步骤2.6：检测并插入关键帧，对于双目会产生新的MapPoints
            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的3D map点
            for(int i=0; i(NULL);
            }
        }

        // Reset if the camera get lost soon after initialization
        // 跟踪失败，并且relocation也没有搞定，只能重新Reset
        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);
    }

处于OK，跟踪局部地图。上一步得到初始位姿，这一步将局部MapPoints和当前帧进行投影匹配以寻找更多匹配点，优化相机位姿。

1、如果局部地图激活(!mbOnlyTracking)跟踪局部地图bOK = TrackLocalMap()，否则在重定位后特征点足够(bOK && !mbVO)也会跟踪局部地图bOK = TrackLocalMap()

2、更新运动模型中的速度mVelocity，mVelocity = mCurrentFrame.mTcw*LastTwc

3、清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）

4、检测（NeedNewKeyFrame()）并插入关键帧CreateNewKeyFrame()

5、保存最新一帧的数据mLastFrame = Frame(mCurrentFrame);

    // 步骤3：记录位姿信息，用于轨迹复现
    if(!mCurrentFrame.mTcw.empty())
    {
        // 计算相对姿态T_currentFrame_referenceKeyFrame
        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 lost
        // 如果跟踪失败，则相对位姿使用上一次值
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState==LOST);
    }

当前帧的相对位姿Tcr=mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();