今天主要是分析一下Tracking.cpp这个文件，它是实现跟踪过程的主要文件，这里主要针对单目，并且只是截取了部分代码片段。
一、跟踪过程分析
- 首先构造函数中使用初始化列表对跟踪状态mState(NO_IMAGES_YET), 传感器类型mSensor(sensor), 是否只进行定位mbOnlyTracking(false)等变量进行了初始化（注意：一些const关键字或者指针类的变量只能使用初始化列表进行初始化），同时在构造函数的函数体内通过OpenCV的FileStorage从文件中读取了Camera标定参数，ORB特征提取的相关参数等，并用其对相关变量进行了初始化。（由于这一段比较简单，很容易看懂，就不再赘述。）
- Tracking.cpp文件的调用接口函数是：
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp)
它完成了对一帧的初始化，并转入Track过程：
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); else mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); Track();
- Track（）实现了跟踪的主要逻辑过程：（1）第一步首先就是判断是否进行了初始化，关于初始化先暂且不表，只知道它通过调用一个初始化函数进行了初始化：
if(mState==NOT_INITIALIZED) { //根据双目或RGBD或单目分别进行初始化，调用不同的函数； if(mSensor==System::STEREO || mSensor==System::RGBD) StereoInitialization(); else MonocularInitialization(); mpFrameDrawer->Update(this); //如何初始化没有完成，需要返回重新进入线程进行初始化，成功了继续执行； if(mState!=OK) return; }
（2）接下来也是判断，这里是同时跟踪和建图的跟踪过程，其实只有跟踪的时候也很有意思：
if(!mbOnlyTracking) { // Local Mapping is activated. This is the normal behaviour, unless // you explicitly activate the "only tracking" mode. //判断系统跟踪状态； if(mState==OK) { // Local Mapping might have changed some MapPoints tracked in last frame CheckReplacedInLastFrame(); //判定速度是否为空，是则根据参考帧进行跟踪，否则根据运动模型进行跟踪，或者距离上一次重定位过去少于2帧； if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2) { bOK = TrackReferenceKeyFrame(); } else { bOK = TrackWithMotionModel(); //如果运动模型跟踪失败，使用参考帧模型进行跟踪； if(!bOK) bOK = TrackReferenceKeyFrame(); } } //如果跟踪状态为丢失，则使用重定位找回当前相机的位姿； else { bOK = Relocalization(); } }
（3）到这里三种跟踪模型都已经出现了，它们分别是：运动模型、参考帧模型、重定位，这里先暂时跳过，后面单独分析这三种模型
TrackWithMotionModel(); TrackReferenceKeyFrame(); Relocalization()从上面的过程可以看出，如果初始化成功或上一帧的状态为成功(mState==OK)，同时速度矩阵非空（mVelocity.empty()），则优先使用运动模型进行跟踪（从后面的分析可以看出这种跟踪方式速度最快），否则使用参考帧模型进行跟踪，如果上一帧跟踪状态为失败，就需要直接进行重定位找回相机位姿。
- 假设不管使用哪种方法，跟踪状态显示成功了，同时返回了一个初始的相机位姿，下面就是要进行局部地图的跟踪过程，可以理解为三种模型获取一个初始相机位姿，然后使用跟踪局部地图的方式对位姿进行优化：
bOK = TrackLocalMap();
我认为只要前面三种模型跟踪成功了，对局部地图的跟踪就会成功，所以这里bOK的状态不会改变（没有考虑其它特殊情况），其结果相当于获得了一个相对精确的相机位姿。
- 下面就是一些扫尾工作，如果不知道运动模型是什么一种跟踪模型，那对前面的速度矩阵肯定很感兴趣了：
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();
结合后面的运动模型可以得知，它假设的是相机运动速度不变（就是假设下一帧的位姿矩阵和这一帧一样，但是相机的位置是不一样的）：
//更新当前帧的位姿：速度乘以上一帧的位姿； mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
其它的工作包括判断是否插入关键帧，删除一些外点，把当前帧置位上一帧等，如果为跟丢，且关键帧总数小于5（初始化不久就丢了），则需要进行重置。
二、子函数分析
- 首先是运动模型：
//设置匹配器；
ORBmatcher matcher(0.9,true);
//更新上一帧信息，对单目只更新了相机位姿； UpdateLastFrame(); //更新当前帧的位姿：速度乘以上一帧的位姿； mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw); fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL)); // Project points seen in previous frame int th; if(mSensor!=System::STEREO) th=15; else th=7; int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
//如果匹配数小于20，则扩大搜索范围；
if(nmatches<20) { fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL)); nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); } //如果还是匹配数小于20，则判定运动模型跟踪失败； if(nmatches<20) return false; //如果匹配数大于20了，就优化相机位姿； // Optimize frame pose with all matches Optimizer::PoseOptimization(&mCurrentFrame);后面就是根据不同情况对跟踪结果进行返回，还有当前帧特征中的地图点的判定等。
- 参考帧模型：
//计算当前帧的Bow向量 mCurrentFrame.ComputeBoW(); //设定匹配器； ORBmatcher matcher(0.7,true); vector<MapPoint*> vpMapPointMatches; //统计当前帧和参考关键帧之间匹配点数，使用BoW加速匹配过程； int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
//这里将上一帧的位姿赋给了当前帧
mCurrentFrame.SetPose(mLastFrame.mTcw);//优化当前位姿；
Optimizer::PoseOptimization(&mCurrentFrame);后面的过程跟运动模型类似。但是这里直接将上一帧位姿作为初值进行优化，并没有使用PnP求解，保留疑问。
- 重定位：
//计算BoW向量； mCurrentFrame.ComputeBoW(); //在关键帧数据库中搜索当前帧的候选关键帧； vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame); if(vpCandidateKFs.empty()) return false; const int nKFs = vpCandidateKFs.size(); //设置匹配器； ORBmatcher matcher(0.75,true); vector<PnPsolver*> vpPnPsolvers; vpPnPsolvers.resize(nKFs); vector<vector<MapPoint*> > vvpMapPointMatches; vvpMapPointMatches.resize(nKFs); vector<bool> vbDiscarded; vbDiscarded.resize(nKFs); int nCandidates=0; //对每一个关键帧进行PnP求解； //首先进行BoW匹配，匹配达到15个点的就进行进一步求解，并作为候选关键帧； for(int i=0; i<nKFs; i++) { KeyFrame* pKF = vpCandidateKFs[i]; if(pKF->isBad()) vbDiscarded[i] = true; else { int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]); if(nmatches<15) { vbDiscarded[i] = true; continue; } else { PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]); pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991); vpPnPsolvers[i] = pSolver; nCandidates++; } } } //再后面一个大循环是上面的匹配结果进行优化求精，进行RANSAC迭代，如果有足够多的内点则跳出循环并返回
重定位的过程至少在逻辑上是很好理解的，就是在已经生成的关键帧数据库中搜索看看当前帧和谁最相近，而方法就是先用BoW匹配，然后进行PnP求解，最后使用RANSAC迭代。
- 还有一个重要的函数，就是局部地图跟踪：
//更新局部地图，包括关键帧和地图点的更新； //它的主要作用就是通过关键帧和地图点的共视关系更新这两个变量：mvpLocalKeyFrames，mvpLocalMapPoints，它们中存储着局部地图中的关键帧和地图点； UpdateLocalMap(); //这个函数就是搜索一下mvpLocalMapPoints中的点是否符合跟踪要求，并匹配当前帧和局部地图点； SearchLocalPoints(); //然后就是优化位姿，需要注意的是，无论是匹配过程还是优化过程都会对地图点做一些修改 Optimizer::PoseOptimization(&mCurrentFrame); //后面就是根据匹配和优化结果更新地图点的状态，并判断匹配的内点数量，最后返回
实际上局部地图匹配的过程要比上面几行代码复杂的多，基本思想就是通过共视关系找出局部地图的关键帧和局部地图点，并用当前帧与之进行匹配优化。
- 还有一些子函数，如判断是否插入关键帧，跟踪的重置，初始化过程，以及只进行跟踪不建图的跟踪过程等，以后有机会再说吧。
三、总结
通过梳理跟踪过程的代码，对ORB-SLAM的跟踪过程的算法和代码都有了更深入的理解，也体会到了写一个类似工程的知识量和工作量。
从前面的分析知道跟踪过程的时间消耗主要在局部地图的跟踪过程，提高效率的方法之一应该就是相应的减小局部地图的大小，具体来说就是mvpLocalKeyFrames，mvpLocalMapPoints这两个变量的大小，现在还没遇到这个问题，所以没有做相应实验。