今天主要是分析一下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這兩個變量的大小，現在還沒遇到這個問題，所以沒有做相應實驗。