其讀取與前文寫的System檢查它所用的方法相同，即構造OpenCV的cv::FileStorage類從而實現讀取，部分代碼如下：

cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
float fx = fSettings["Camera.fx"];
float fy = fSettings["Camera.fy"];


（1）使用上述方法先讀取到相機內參fx、fy、cx、cy，這樣相機的內參矩陣K就被確定了。（2）讀取相機的徑向畸變參數k1、k2、k3以及切向畸變參數p1、p2。（3）讀取相機的參數bf，該參數實際是「雙目相機基線b × 相機內參fx」（根據雙目相機模型，深度d=bf/視差）。（4）讀取相機幀速率fps。（5）讀取參數RGB（顏色存儲順序是RGB而不是BGR）。（6）每張圖提取的特徵數nFeatures，圖像金字塔每層間的尺度因子scaleFactor，圖像金字塔層數nLevels，提取FAST角點時所使用的初始響應值閾值iniThFAST，還有如果提取角點所用的響應值的初始閾值iniThFAST過高則還有一個最小閾值參數minThFAST。（7）對於採用雙目相機的情況，ORB-SLAM2系統根據特徵點對應的深度值大小區分了近點與遠點，所用的閾值為ThDepth（近點的位移信息更準確，僅需要簡單的三角化來初始化對應的地圖點）。

2）Tracking::GrabImageStereo

cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp)


根據前文，該函數是跟蹤線程的核心，其被System::TrackStereo調用，輸入的左右圖與時間戳信息獲取當前幀坐標系相對於世界坐標系的變換矩陣Tcw。

首先該函數把輸入的左右圖分別轉為灰度圖保存為Tracking的成員變數mImGray、mImGrayRight。

然後利用左右灰度圖與時間戳信息構造一個Frame類對象（http://Frame.cc），其保存為Tracking的成員變數mCurrentFrame。

接下來執行Tracking類的核心函數：Track()。

Track()函數中將得到當前幀的Tcw，也就是mCurrentFrame的成員變數mTcw，其將被本函數返回。

綜上，本函數的核心其實是調用了Frame的構造函數以及Tracking::Track()。

3）Tracking::Track()

void Tracking::Track()


上一個函數中說了，本函數是Tracking類的核心。

本函數主要工作：初始化——初始跟蹤估計相機位姿——跟蹤局部地圖——決定新的關鍵幀——當前幀相關信息保存。運行有兩種模式：只跟蹤、跟蹤且建圖。

Tracking構造函數剛剛構造完的時候，Tracking的成員mState為NO_IMAGES_YET。這樣的情況下，本函數首先把mState改為NOT_INITIALIZED，成員mLastProcessedState改為mState。接下來鎖住地圖的更新，進行後續的工作。

第一步：初始化。

雙目情況的初始化直接調用 Tracking::StereoInitialization() 函數進行。初始化之後通過「mpFrameDrawer->Update(this)」更新一下FrameDrawer。初始化的最後再檢查一下mState：如果狀態不是OK，那麼本函數直接return結束運行；Tracking的狀態OK則繼續後續工作。

第二步：通過跟蹤初始估計相機位姿。

ORB-SLAM2系統並不是我們想的那樣直接通過特徵點與點雲（地圖點）的匹配計算出位姿。實際情況是：每一幀對應的初始相機位姿估計是通過運動模型或者跟蹤參考關鍵幀得到的（如果跟蹤失敗，則位姿估計是通過重定位得到的）。

• 跟蹤且建圖模式

跟蹤且建圖模式下局部建圖是被激活的。

如果Tracking的狀態mState不是OK，直接調用 Tracking::Relocalization() 進行重定位，否則才進行一般的跟蹤。跟蹤之前，由於局部建圖線程可能改變前一幀中被跟蹤的地圖點，因此先調用了 Tracking::CheckReplacedInLastFrame() 更新一下點雲。一般的跟蹤也有兩種情況。跟蹤情況①：沒有對於速度的估計（mVelocity為空），或者距離上一次重定位才過去不超過1幀。跟蹤情況②：情況①的否定。

跟蹤情況①：調用 Tracking::TrackReferenceKeyFrame() 跟蹤當前幀的參考關鍵幀。

跟蹤情況②：調用 Tracking::TrackWithMotionModel() 通過運動模型跟蹤。如果使用運動模型後匹配的點數不夠多（之後具體解釋），說明運動模型是失效的，那麼還是跟情況①一樣跟蹤參考關鍵幀。

• 只跟蹤不建圖模式

該模式下如果mState是LOST就直接重定位，否則開始跟蹤。跟蹤根據Tracking的成員變數mbVO是否為true分為兩種。mbVO為true說明在只跟蹤模式下，當前幀與上一幀基本沒匹配點對。

mbVO=true。這種情況同時進行運動模型跟蹤（如果mVelocity不為空）與重定位。如果不能用運動模型跟蹤或者運動模型不可靠但是重定位結果好，那麼就用重定位結果且mbVO改為false；否則就用運動模型的跟蹤結果（這一步mbVO一直保持為true），並且對於採用運動模型跟蹤時觀察到的不是外點的地圖點，這些地圖點的成員變數mnFound要+1（說明其又被新的一幀觀察到了）。當然，也有可能重定位和運動模型跟蹤的結果都不好，這在後面會進行處理。

mbVO=false。這種情況下有速度就用運動模型，否則跟蹤參考關鍵幀。

以上就是跟蹤來進行初始的相機位姿估計的步驟，其實就只有三種可能操作：用運動模型跟蹤、用參考關鍵幀跟蹤、重定位。需要說明的是，這三種操作都會返回一個bool值表示其結果是否可接受，記為bOK（實際採用的那個的返回值是本函數中的bOK）。

第三步：跟蹤局部地圖。

在跟蹤且建圖情況下，如果bOK為真，則調用 Tracking::TrackLocalMap() 跟蹤局部地圖；在只跟蹤情況下，只有bOK為真且mbVO為假（mbVO為真則說明跟局部地圖也沒有什麼匹配的點，無法跟蹤局部地圖）時才調用 Tracking::TrackLocalMap() 跟蹤局部地圖。因此，跟蹤局部地圖就是調用 Tracking::TrackLocalMap() 這個函數，該函數也會返回一個bool值表示跟蹤結果是否令人滿意，其將被作為新的bOK。

如果跟蹤局部地圖後bOK為真，則Tracking的狀態mState為OK；否則mState狀態為LOST。需要注意的是，有可能跟蹤估計位姿那一步結果就不好因此沒有進行後續的局部地圖跟蹤，該情況也會有bOK=false，因此mState也會是LOST。

接下來再次更新一下mpFrameDrawer。接著，如果bOK=true，本函數第四步開始決定要不要創建新的關鍵幀（前面說過ORB-SLAM2是基於關鍵幀優化的SLAM系統，這裡開始將挑出關鍵幀）。

第四步：檢查是否創建新的關鍵幀。（前提是bOK=true，說明跟蹤正常）

在真正決定是否創建新關鍵幀前先做一些其他的工作。

（1）更新運動模型的速度。Tacking的成員mLastFrame記錄了上一個被處理的幀，如果mLastFrame的位姿(mTcw)為空則無法獲得當前速度估計（mVelocity為空）；否則通過mLastFrame.mTcw計算出運動模型的速度mVelocity（速度 = 當前幀的Tcw × 上一幀的Twc）。這裡我順便來理解一下運動模型的速度mVelocity。上一幀和當前幀位姿知道後我們就能估計一個速度，這個速度可以在Tracking線程跟蹤下一幀時使用。我的理解是這個速度實際是相同時間間隔（相機幀率是固定的）內相鄰兩幀坐標系之間的變換矩陣（等式變為：速度 × 上一幀的Tcw = 當前幀的Tcw），也就是說系統會試圖用上一幀與當前幀之間的變換矩陣作為當前幀和下一幀之間的變換矩陣，這樣等於認為相同時間間隔內的變換矩陣是相同的，也就被稱為勻速運動模型。這一步結束後，mpMapDrawer把當前幀位姿mTcw設置為當前的相機位姿。

（2）首先需要說明一些新內容。ORB-SLAM2的關鍵是提取每一幀中的ORB特徵點，這是系統運行的基礎（我們真正利用的信息僅僅是特徵點，因此系統的建圖也是稀疏的）。每個Frame類會把其提取出的關鍵點的數量記為N（1~N同時也作為了這些關鍵點的索引）。每個Frame類還有一個成員mvpMapPoints，其容量也是N。如果對應索引（例：第 i 個）的關鍵點有匹配的地圖點，則那個地圖點的指針會被存到對應關鍵點索引的位置上（例：mvpMapPoints[ i ] ）；否則 mvpMapPoints[ i ] 就會是空的。Frame類還有一個成員mvbOutlier，它每一位表示對應索引的關鍵點的匹配到的地圖點是否是外點（沒有對應地圖點則為false）。第（2）步中，檢查當前幀的每個關鍵點對應的地圖點：如果存在該對應地圖點且沒有關鍵幀觀察到它（地圖點的成員nObs<1），則這個對應地圖點從mvpMapPoints中刪去（對應位置為空的MapPoint*），mvbOutlier對應位置為false。

（3）清理臨時地圖點。臨時地圖點保存在Tracking的成員 mlpTemporalPoints 之中，這裡直接清空。所謂的臨時地圖點是指 Tracking::UpdateLastFrame 函數（TrackWithMotionModel函數中被調用）中人工補充的地圖點。

然後開始檢查是否需要新關鍵幀（Tracking::NeedNewKeyFrame()返回true，這一步條件比較寬鬆，因為局部建圖線程還會剔除一些關鍵幀）。如果需要則調用Tracking::CreateNewKeyFrame() 創建新的關鍵幀。

然後線程會把當前幀關鍵對應的是外點的地圖點清空了。這個操作就是通過mCurrentFrame的mvpMapPoints和mvbOutlier完成的，可參考第四步（2）中說明。為什麼在創建關鍵幀之後才清除是外點的地圖點？根據代碼的注釋，關鍵幀將在後續進行集束優化，該優化將決定那些點是否真的是外點，因此不急著清除。而清除這些地圖點的主要原因是不希望它們影響下一幀的位姿估計。

第五步：當前幀相關信息保存在Tracking的成員中。

最後保存一下當前幀的位姿等信息（前面分析過的保存相機軌跡要用）。如果當前幀的mTcw已經得到了，則進行以下操作：（1）計算當前幀相對於參考關鍵幀的變換Tcr（Tcr = Tcw × Twr），Tcr被推入Tracking的成員 mlRelativeFramePoses 中保存；（2）當前幀的時間戳推入Tracking的成員 mlFrameTimes 中保存；（3）當前幀的參考關鍵幀推入Tracking的成員 mlpReferences 中保存；（4）當前幀是否跟蹤失敗的bool信息（mState==LOST）推入Tracking的成員 mlbLost 中保存。當然跟蹤失敗的時候當前幀的mTcw未知，則前面（1）（2）（3）步都只在尾部重複保存對應三個列表的尾部的元素，然後mlbLost尾部保存true。

3）Tracking::StereoInitialization

void Tracking::StereoInitialization()


前面Track()函數分析中剛開始的初始化就調用了本函數。本函數主要工作（條件是當前幀關鍵點足夠多）：當前幀設置為起始幀——當前幀構造為第一個關鍵幀——創建初始的點雲地圖——相關後處理。

本函數要起作用的條件是：當前幀中關鍵點數量N>500。如果這個條件不滿足，本函數不起作用；對於Track()函數來說，它將跳過當前幀，對之後的幀繼續嘗試初始化，直到初始化成功為止。本條件滿足後，函數開始進行後續工作。

第一步：當前幀被設置為第一幀。

初始化就是在處理第一幀，該幀將被作為跟蹤的起點。本步驟的實際操作是：將當前幀的位姿Tcw設置為單位矩陣。也就是說當前幀無旋轉、無位移（畢竟它是起點嘛）。

第二步：當前幀構造為關鍵幀。

直接使用KeyFrame的構造函數構造第一個關鍵幀，本函數將得到該關鍵幀的指針。由於地圖類記錄著所有關鍵幀的指針，因此把第一個關鍵幀加入到Tracking::mpMap中（代碼為mpMap->AddKeyFrame(pKFini)）。

第三步：創建初始的地圖點並建立必要的數據關聯。

該步驟將遍歷當前幀的所有關鍵點，通過深度z>0的關鍵點創建出對應地圖點，具體步驟如下。（1）通過當前幀的 UnprojectStereo 方法將第 i 個關鍵點反投影得到三維坐標 x3D (cv::Mat)。（2）構造新的MapPoint（構造函數輸入x3D、初始關鍵幀指針、地圖指針mpMap），得到該地圖點指針 pNewMP (MapPoint*)。（3）地圖點記錄下初始關鍵幀觀察到了自己（pNewMP->AddObservation(pKFini,i)），初始關鍵幀記錄下自己觀察到了該地圖點（pKFini->AddMapPoint(pNewMP,i)），地圖點通過自己的 ComputeDistinctiveDescriptors() 方法計算最好的描述子（初始化中只有一個初始的關鍵幀觀察到它，因此就使用地圖點在該關鍵幀中的描述子）。（4）該地圖點使用其 UpdateNormalAndDepth() 方法計算自己平均被觀察的方向以及尺度不變距離。（5）該地圖點加入到地圖當中（mpMap->AddMapPoint(pNewMP)）。（6）當前幀也記錄了該地圖點，這條記錄實際也通過索引與該地圖點的對應關鍵點相關聯（mCurrentFrame.mvpMapPoints[ i ]=pNewMP）。

一個初始的地圖（很稀疏的點雲）將通過第三步建立起來。

第四步：相關後處理。

（1）首先第一個關鍵幀得送到局部建圖線程中去。（2）更新Tracking類的「上一…」信息：當前幀通過一個Frame構造函數初始並設置為Tracking類的上一幀mLastFrame，當前幀的ID（mnID）設置為上一個關鍵幀的ID（mnLastKeyFrameId），構造的第一個關鍵幀記錄為上一個關鍵幀（mpLastKeyFrame）。（3）Tracking類的局部地圖（不是地圖）的相關數據記錄：初始關鍵幀推入mvpLocalKeyFrames 保存，當前已有的初始點雲保存為 mvpLocalMapPoints，初始關鍵幀保存為當前Tracking的參考關鍵幀成員 mpReferenceKF。（4）當前幀的參考關鍵幀設為根據自己創建的初始關鍵幀，地圖的參考地圖點 mvpReferenceMapPoints 設置為前面保存的局部地圖點，初始關鍵幀被推入地圖的 mvpKeyFrameOrigins 保存。（5）當前幀的位姿Tcw作為當前相機位姿傳遞給MapDrawer。（6）Tracking的狀態eState設為OK。

4）Tracking::CheckReplacedInLastFrame()

void Tracking::CheckReplacedInLastFrame()


本函數遍歷上一幀所有關鍵點所對應的地圖點（保存在 mLastFrame.mvpMapPoints 中），檢查一下地圖點是否存在對應的替換點（地圖點的 GetReplaced() 方法是否可以返回一個地圖點指針）。局部建圖線程可能會使一些地圖點存在對應的替換點，如果有替換點，就用其替換原本的上一幀地圖點。

5）Tracking::TrackReferenceKeyFrame()

bool Tracking::TrackReferenceKeyFrame()


如前文所述，本函數完成的跟蹤參考關鍵幀工作是跟蹤估計幀位姿的方法之一。其大致步驟為：當前幀與參考關鍵幀通過詞袋快速匹配——當前幀記錄成功匹配的地圖點——PnP優化——處理地圖點中的外點。

第一步：首先在當前幀與參考關鍵幀之間進行ORB關鍵點與地圖點的匹配。

ORB-SLAM2系統中的ORB匹配都會通過詞袋方法加速。（1）使用當前幀的 ComputeBoW() 方法計算當前幀的詞袋向量。（2）使用對應構造函數初始化一個matcher（ORB_SLAM2::ORBmatcher ），使用其方法 SearchByBoW 在Tracking類的當前幀mCurrentFrame以及當前參考關鍵幀mpReferenceKF之間。對於匹配成功的當前幀的關鍵點，其對應參考關鍵幀的地圖點的指針將被保存在向量 vpMapPointMatches中（與Frame的mvpMapPoints一樣的形式，依次對應當前幀的關鍵點，若無對應則相應位置上元素為NULL）。（3）如果匹配點對少於15對，則本函數返回false（也就是前文提到的bOK為false，說明跟蹤參考關鍵幀失敗了）。

如果這一步的匹配成功，當前幀將把匹配的地圖點 vpMapPointMatches 保存為自己的成員 mvpMapPoints。

第二步：使用PnP演算法優化估計位姿。

首先將當前幀的位姿設置為上一幀的位姿Tcw，然後使用下面的代碼對當前幀進行PnP位姿優化（具體的代碼在http://Optimizer.cc中）。

Optimizer::PoseOptimization(&mCurrentFrame);


第三步：拋棄匹配地圖點中的外點。

第二步的優化會決定匹配成功的那些地圖點是否為外點（該bool信息保存在當前幀的mvbOutlier之中），因此跟蹤的最後要拋棄掉外點。此過程與前文提到的 Track() 中處理外點的方法一樣，是外點就把當前幀的mvpMapPoints對應位置改為空指針，mvbOutlier對應位置改回false。

另外，該地圖點的成員mbTrackInView應改為false，表示這個地圖點不再當前Tracking的視野之內；該地圖點的成員 mnLastFrameSeen 改為當前幀的ID，表示最後跟丟這一個地圖點時對應的幀是當前幀。

拋棄完外點之後最終可以確定匹配點數，如果其>=10，則跟蹤成功並返回true；否則跟蹤當前參考關鍵幀失敗，返回false（bOK=false）。

6）Tracking::UpdateLastFrame

void Tracking::UpdateLastFrame()


該函數將在使用運動模型跟蹤時被調用，根據上一幀的參考關鍵幀更新上一幀的位姿（處於只跟蹤模式且上一幀沒有對應關鍵幀，則補全上一幀的一些「近關鍵點」對應的地圖點作為臨時地圖點）。

第一步：根據上一幀的參考關鍵幀更新上一幀的位姿。

（1）從Tracking的 mlRelativeFramePoses 尾部獲得上一幀的參考關鍵幀到上一幀相對變換矩陣 Tlr。（2）上一幀的位姿 = Tlr × 上一幀的參考關鍵幀的位姿 Trw。

這一步完成後，如果系統滿足「不是只跟蹤模式、或是單目感測器情況、或是上一幀被創建了關鍵幀」，則直接結束本函數；否則進行後續步驟。

第二步：創建「視覺里程計」的地圖點（臨時地圖點）。

首先將上一幀的關鍵點按照深度從小到大的順序排序（深度必須為正）。如果上一幀的關鍵點對應的地圖點不存在或者存在但沒有被任何關鍵幀觀察到，則創建對應的「視覺里程計」的地圖點（臨時地圖點）。

接下來創建臨時地圖點，方法與Track()初始化時創建初始地圖點方法一致：反投影獲得三維坐標 x3D 後用構造函數生成新地圖點。稍微有區別的是，這一步中創建新地圖點時構造函數中輸入了其對應關鍵點的索引 i ，另外該地圖點僅保存進上一幀的 mvpMapPoints 以及Tracking的成員 mlpTemporalPoints（本步驟僅是補充臨時地圖點）之中。

該創建臨時地圖點的步驟停止條件：當前正在處理的關鍵點已經是遠點（深度大於Tracking.mThDepth）並且有地圖點（包括臨時地圖點）的關鍵點已經大於100個。

7）Tracking::TrackWithMotionModel

bool Tracking::TrackWithMotionModel()


本函數使用「勻速」運動模型估計當前幀位姿，主要工作為：更新上一幀位姿——當前幀位姿用運動模型的假設去計算更新——用當前幀去匹配上一幀的地圖點——優化——處理外點。

第一步：調用UpdateLastFrame()更新上一幀位姿（可能創建上一幀的臨時地圖點）。

第二步：使用運動模型計算當前幀位姿（當前幀Tcw = Tracking.mVelocity * 上一幀位姿Tcw）。

第三步：將上一幀的地圖點（可能包括第一步添加的臨時地圖點）投影至當前幀並與關鍵點匹配。（1）清空當前幀的地圖點（即mvpMapPoints中全部為空指針）。（2）創建ORBmatcher並使用其 SearchByProjection 方法進行上一幀地圖點與當前幀關鍵點之間的匹配。

這一步的匹配需要設置閾值th，該閾值控制上一幀地圖點投影到當前幀並在附近搜索關鍵點的範圍大小（th越小肯定越難匹配到對應關鍵點）。第一次匹配i使用的th=15，若匹配點對少於20個則調整th=30並再次匹配；若第二次匹配點對還少於20則本函數返回false，通過運動模型跟蹤失敗。

第三步：根據匹配點對進行當前幀位姿優化。前面使用運動模型估計的位姿肯定是不準確的，這一步使用匹配點對進行位姿優化，方法同跟蹤參考關鍵幀（調用 PoseOptimization ）。不同的是：跟蹤參考關鍵幀時不會首先估計一個位姿再優化。

第四步：拋棄外點。

這一步與 Tracking::TrackReferenceKeyFrame() 中拋棄外點的方法一致，可以參考前文。

最後判斷一下本次跟蹤狀態。

如果Tracking處於只跟蹤模式，那麼如果當前幀有效的地圖點（不是外點且有關鍵幀觀察到它）少於10個，則Tracking的 mbVO 被設置為true；如果當前幀有效的地圖點多於20個，則本次跟蹤成功。如果Tracking處於跟蹤且建圖模式，那麼只要當前幀有效地圖點多於9個即跟蹤成功，本函數返回true。

8）Tracking::TrackLocalMap()

bool Tracking::TrackLocalMap()


Track()通過跟蹤前一幀或參考關鍵幀（或重定位）獲取到當前幀位姿的估計之後，會調用本函數來跟蹤局部地圖。本函數主要工作流程為：更新局部地圖——當前幀與局部地圖匹配——優化當前幀位姿——更新當前幀匹配到的地圖點的信息——根據匹配內點數決定匹配是否成功（剛剛重定位的話條件更嚴格）。

第一步：獲取局部點雲地圖。

這一步實際使用 Tracking::UpdateLocalMap() 函數更新Tracking維護的局部地圖（mvpLocalMapPoints（同時設置為mpMap的參考地圖點mvpReferenceMapPoints）、mpReferenceKF（同時也是當前幀的參考關鍵幀））。

第二步：當前幀關鍵點與局部地圖中地圖點匹配。

這一步使用 Tracking::SearchLocalPoints() 在當前幀與局部地圖之間進行點的匹配。

第三步：當前幀位姿優化。

與運動模型估計與跟蹤參考幀時一樣，採用 Optimizer::PoseOptimization(&mCurrentFrame) 優化當前幀位姿。

第四步：更新地圖點的數據，決定本次跟蹤局部地圖是否成功。

遍歷當前幀跟蹤到的地圖點 mvpMapPoints，如果是外點則拋棄；如果不是外點則該地圖點的 mnFound +1。另外，對於不是外點的地圖點：如果只跟蹤模式，則匹配內點數（ Tracking.mnMatchesInliers ）+1；否則如果該點被關鍵幀觀察到，則匹配內點數+1。

（1）當前幀ID<mnLastRelocFrameId+幀率mMaxFrames（該條件表示剛剛重定位過）的情況下：如果匹配點對數量不足50則本次跟蹤局部地圖失敗，返回false。

（2）一般情況下或（1）沒有直接返回，那麼匹配點對數小於30返回false，否則跟蹤成功返回true。

9）Tracking::NeedNewKeyFrame()

bool Tracking::NeedNewKeyFrame()


Tracking線程需要在尾部決定是否創建新的關鍵幀，本函數就完成了此判斷。本函數的判斷結果主要取決於當前跟蹤效果是否理想、當前幀的跟蹤質量好壞、當前局部建圖線程是否有空。

如果Tracking處於只跟蹤模式，則本函數直接返回false，不需要新關鍵幀；如果局部建圖線程因為檢測到了閉環而被終止或正在終止，則本函數直接返回false；如果距離上一次重定位不久，直接返回false。

接下來的判斷較為複雜。首先建立了幾個自條件是否滿足的bool標誌：c1a：距離上一次插入關鍵幀過去了mMaxFrames以上；c1b：距離上一次插入關鍵幀僅僅過去mMinFrames以上並且局部建圖線程空閑；c1c：當前跟蹤很弱(當前幀匹配的內點遠少於參考關鍵幀中被觀察數較多的地圖點的數量) ，或Tracking現在需要插入更多的近地圖點（當前幀有匹配地圖點的近關鍵點很少並且還有很多近關鍵點沒有匹配到地圖點）；c2a：當前跟蹤較弱(當前幀匹配的內點較少於參考關鍵幀中被觀察數較多的地圖點的數量) ，或Tracking現在需要插入更多的近地圖點（當前幀有匹配地圖點的近關鍵點很少並且還有很多近關鍵點沒有匹配到地圖點）；c2b：當前幀匹配的是內點的地圖點數量大於15個。

後續判斷必須滿足條件： (c1a || c1b || c1c) && c2a && c2b。注意，c2b必須滿足，因為只有滿足了c2b才能說明當前幀有價值作為新關鍵幀，而其他的條件是說明我們可以插入新關鍵幀或者跟蹤效果不好必須要插入關鍵幀。該條件不滿足則直接返回false，滿足則進行後續判斷。

①局部建圖線程空閑：直接返回true，決定創建新關鍵幀。

②局部建圖線程不空閑：首先讓局部建圖線程去打斷集束優化操作（mpLocalMapper->InterruptBA()）。最後，如果局部建圖線程的關鍵幀隊列中關鍵幀數量小於3則返回true，否則返回false。

10）Tracking::CreateNewKeyFrame()

void Tracking::CreateNewKeyFrame()


該函數主要工作流程：停止局部地圖——當前幀創建新KF——補全當前幀近關鍵點對應地圖點——新關鍵幀插入局部建圖——局部建圖繼續。

第一步：停止局部建圖線程。

首先通過「mpLocalMapper->SetNotStop(true)」停止局部建圖，如果不成功則本函數直接返回；否則繼續後面的步驟。

第二步：將當前幀構造為關鍵幀。

使用關鍵幀的構造函數將當前幀構造為關鍵幀，並將其設置為Tracking類和當前幀的「參考關鍵幀」。然後更新一下當前幀的旋轉矩陣、位移向量、相機光心的世界坐標（Frame::UpdatePoseMatrices）。

第三步：補充創建當前幀近關鍵點對應的地圖點（當前幀和新關鍵幀都將擁有該地圖點）。

首先檢查是否需要創建新的地圖點（按照關鍵點深度從小到大）：對於深度大於0的當前幀關鍵點，如果其沒有對應地圖點或者地圖點沒有被觀察到（Observation<1），則創建新的地圖點（與前面創建方法一樣Frame::UnprojectStereo + MapPoint構造函數）並更新信息。

以上補充創建地圖點的過程在滿足下述條件時停止：關鍵點深度大於mThDepth且當前幀有100個以上地圖點。

第四步：新關鍵幀送入局部建圖線程。

該步驟通過「mpLocalMapper->InsertKeyFrame(pKF)」完成。隨後局部建圖線程繼續運行，並將Tracking的mnLastKeyFrameId設置為當前幀ID、mpLastKeyFrame設置為新創建的關鍵幀。

11）Tracking::SearchLocalPoints

void Tracking::SearchLocalPoints()


該函數在 Tracking::TrackLocalMap() 函數中被調用，用來完成當前幀關鍵點與局部地圖點之間的匹配。該函數先統計有沒有局部地圖點存在被匹配的可能，存在這種點的話再調用 SearchByProjection 進行地圖點的投影以及匹配。

第一步：標記已經被當前幀匹配好的地圖點（之後不需要再搜索匹配）。

這一步通過遍歷當前幀匹配的地圖點mvpMapPoints來標記每個被當前幀匹配好的地圖點（地圖點的成員 mnLastFrameSeen 改為當前幀ID），這些地圖點已經匹配成功之後不需要再匹配。

第二步：統計可能可以進行匹配的局部地圖點的數量。

遍歷Tracking線程跟蹤的局部地圖點 mvpLocalMapPoints ，跳過那些壞點（pMP->isBad()）以及第一步被標記的已經匹配的點，將在當前幀視錐範圍內（ mCurrentFrame.isInFrustum(pMP,0.5) ）的地圖點統計作為想要匹配的局部地圖點。如果這些可以匹配的局部地圖點數量大於零則進行後續步驟，否則本函數結束。

第三步：通過投影匹配當前幀關鍵點與局部地圖點。

首先創建ORBmatcher，然後設置搜索範圍的閾值為1（如果距離上一次重定位不到兩幀則閾值為5），最後調用ORBmatcher的 SearchByProjection 方法（輸入就是當前幀、局部地圖點、搜索範圍閾值）進行匹配。

12）Tracking::UpdateLocalPoints()

void Tracking::UpdateLocalPoints()


該函數被 Tracking::UpdateLocalMap() 調用，後者又被 Tracking::TrackLocalMap() 在函數的開頭調用。這個函數的本質是通過局部關鍵幀得到最新的局部地圖點。

第一步：清空當前Tracking的局部地圖點（mvpLocalMapPoints）。

第二步：當前局部關鍵幀對應的地圖點作為局部地圖點保存。

遍歷當前的局部關鍵幀 mvpLocalKeyFrames ，對於每一關鍵幀都要遍歷其地圖點並將其推入 mvpLocalMapPoints 保存（注意通過標記 mnTrackReferenceForFrame 改為當前幀ID避免重複添加）。

13）Tracking::UpdateLocalKeyFrames()

void Tracking::UpdateLocalKeyFrames()


該函數同樣被 Tracking::UpdateLocalMap() 調用，後者又被 Tracking::TrackLocalMap() 在函數的開頭調用。

本函數流程：先得到最初的局部關鍵幀（與當前幀共視地圖點的關鍵幀）——加入最初局部幀的連接權重高的相鄰關鍵幀——加入最初局部幀的子女、父母。觀察到當前幀的地圖點最多的初始的局部關鍵幀成為：當前幀和Tracking對象的參考關鍵幀。

注意函數的關鍵點在於：局部關鍵幀一定是和當前幀有關聯。

第一步：每個當前幀關鍵點對應的地圖點給觀察到他們的關鍵幀「投票」。

這一步先遍歷當前幀關鍵點對應的地圖點完成該投票，這樣可以知道哪些關鍵幀和當前幀有關聯。投票信息被保存在 keyframeCounter （map<KeyFrame*,int>）當中，每個元素對應（某一幀，相應票數）。如果該投票計數器是空的，則本函數直接返回。說明一下，觀察到某一地圖點的關鍵幀可以通過地圖點的 GetObservations() 方法獲得。

第二步：保存所有與當前幀看到共同地圖點的關鍵幀，將共視點最多的關鍵幀的指針同時保存為pKFmax。

這一步根據第一步的投票結果進行，與當前幀有共視地圖點的關鍵幀都被作為局部關鍵幀保存到 mvpLocalKeyFrames 。保存過的關鍵幀都把自己的成員 mnTrackReferenceForFrame 改為當前幀ID。另外，與當前幀共視程度最高的關鍵幀被單另記了下來。

第三步：第二步得到的局部關鍵幀在系統共視圖中的相鄰關鍵幀也作為局部關鍵幀保存。

這裡終於第一次直接碰到了「共視圖（ Covisibility Graph ）」這個ORB-SLAM1/2系統中十分重要的概念。簡單說明一下，顧名思義，該圖的節點是關鍵幀，該圖的邊的權重是其連接的兩關鍵幀之間共同觀察到的地圖點的數量（邊連接著有共視地圖點的兩關鍵幀）。詳細內容可以參考ORB-SLAM（1不是2）的論文。

後續步驟主要在對第二步得到的局部關鍵幀的大循環中進行，每一次循環前需要限制局部關鍵幀數量，如果已經夠80張的話本函數將直接返回。

遍歷第二步得到的局部關鍵幀，通過關鍵幀的 GetBestCovisibilityKeyFrames() 方法獲取與正在遍歷的關鍵幀共視點最多的相鄰關鍵幀，這裡向該方法輸入10指定獲取共視程度最高的前10張關鍵幀（也可能不夠10張）。遍歷當前正在遍歷的局部關鍵幀的這幾張相鄰關鍵幀，一旦碰到好關鍵幀並且之前沒碰到的（其mnTrackReferenceForFrame不是當前幀ID）就把它當做局部關鍵幀保存並將 mnTrackReferenceForFrame 改為當前幀ID，隨後跳出對這些相鄰關鍵幀的遍歷（也就是說之前的一張局部關鍵幀最多只能再帶進來一張相鄰關鍵幀）。

第四步：第二步得到的局部關鍵幀的子女關鍵幀、父母關鍵幀加入局部關鍵幀。

對於所有的關鍵幀而言，其背後實際上有一棵生成樹，也就是說：關鍵幀有對應的父母關鍵幀與子女關鍵幀。這一步還在對最初的局部關鍵幀的遍歷中，首先得到正在遍歷的關鍵幀的子女關鍵幀（）。

const set<KeyFrame*> spChilds = pKF->GetChilds();


遍歷這個set，如果有子女關鍵幀是好的並且還沒加入到局部關鍵幀，那麼將其加入局部關鍵幀並且標記（mnTrackReferenceForFrame 改為當前幀ID）然後退出遍歷。也就是說，每個初始關鍵幀同樣只能帶一個娃（也可能沒娃）。同理也將正在遍歷的關鍵幀的父母幀保存下來，這裡不再贅述。

最後一步：與當前幀共視地圖點最多的初始的局部關鍵幀（pKFmax指著的）設置為Tracking與當前幀的參考關鍵幀。

14）Tracking::Relocalization()

bool Tracking::Relocalization()


本函數用於跟蹤丟失時進行重定位。

第一步：獲取重定位的候選關鍵幀。

之前提到詞袋方法的時候說起過，該方法就是被用於閉環檢測和重定位時的位置識別方法。因此首先通過當前幀的 ComputeBoW() 方法計算當前幀的詞袋向量表示。

說是位置的識別，具體而言，在基於關鍵幀優化的SLAM系統中，位置的識別實際是找到對應同一位置的關鍵幀。基於圖像的詞袋向量表示，可能對應同一位置的候選關鍵幀（多個候選，指針存在向量中）將從前文提到過的關鍵幀資料庫中查詢得到，代碼如下。如果沒有候選，則函數直接返回。

vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);


第二步：通過ORB匹配確定是否為可靠的候選關鍵幀，對可靠的候選關鍵幀建立 PnPsolver 。

這一步我們篩選一下前面得到的可能對應著當前幀位置的候選關鍵幀，基本思想為：對應同一位置的幀之間應該有很多匹配的點對（畢竟觀察著同一位置）。首先創建了一些要用的變數：一個ORBmatcher用來進行匹配；一個向量的向量 vvpMapPointMatches 用來保存每個候選關鍵幀的與當前幀匹配成功的地圖點的指針；一個bool的向量 vbDiscarded 用來保存對應索引的候選關鍵幀是否被拋棄。

然後真正開始遍歷第一步得到的候選關鍵幀，對於第 i 張關鍵幀，使用ORBmatcher的 SearchByBoW 方法進行其與當前幀之間的匹配，並將匹配成功的地圖點的指針存入 vvpMapPointMatches[ i ] 。對於匹配點數少於15的候選關鍵幀，其對應的 vbDiscarded 中元素設為true，其將被拋棄；其他的關鍵幀即為合格，將為他們建立對應的PnPsolver（輸入當前幀與匹配地圖點的向量）並將PnPsolver對應指針存入 vpPnPsolvers中。本函數中使用一個變數 nCandidates 記錄了合格的候選關鍵幀的數量。

第三步：通過匹配地圖點優化當前幀位姿，確定最終的重定位關鍵幀。

如果 nCandidates 大於0並且還沒有確定最終的重定位關鍵幀（一個bool變數 bMatch 仍為false），那麼將一直進行一個大循環。在這個大循環中遍歷之前的候選關鍵幀，步驟如下。

（1）跳過被標記為拋棄的關鍵幀。

（2）對合格的關鍵幀利用PnPsolver進行5輪Ransac迭代計算出一個位姿Tcw，該步驟代碼為：

cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);


輸入的 bNoMore 表示 Ransac 是否達到最大，如果其為true則該關鍵幀也將被標記拋棄並且 nCandidates 減1；輸入的 vbInliers 標記之前候選關鍵幀匹配到的地圖點是否為內點、nInliers 統計了內點數量。對於是內點的地圖點，存入了一個循環中的變數sFound（std::set）。

（3）如果通過上面這行代碼我們真的得到了一個位姿，那麼將其設置為當前幀位姿。對於匹配的地圖點，將其全部設置為當前幀的地圖點，也就是 mvpMapPoints（當然不是內點的地圖點指針修改為NULL）。也就是說在重定位的情況下地圖點不是像正常情況那樣反投影創建並得到地圖點的。

（4）繼續得到了位姿的條件下，當前幀已經有了地圖點，這樣我們進行一次當前幀的位姿優化，代碼如下：

int nGood = Optimizer::PoseOptimization(&mCurrentFrame);


nGood表示優化結束後是內點的當前幀匹配的地圖點數量，如果其小於10直接進行下一次循環（檢驗下一張候選關鍵幀）。需要注意的是：本次包括之後的位姿優化後有些當前幀的地圖點會成為外點，因此優化完需要清除是外點的地圖點的指針。

（5）如果nGood小於50大於10，通過在一個窗口內投影並搜索匹配，再次進行優化。這一步的匹配用一個條件較為寬鬆些的ORBmathcer進行，使用其 SearchByProjection 方法，代碼如下。

int nadditional = matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);


輸入了當前幀、候選關鍵幀、已經匹配的是內點的地圖點。其中 10 是控制搜索範圍大小的變數，100是控制描述子匹配閾值的變數。返回的 nadditional 表示這次額外加入的內點地圖點的數量，如果其加上nGood還是不夠50則再次重複這一步（不過用跟小的閾值3,64），然後重複之前進行的優化步驟並得到新的nGood。

（6）步驟（4）或（5）中得到nGood一旦夠50則認為優化是成功的，bMatch為true（確定了重定位的對應關鍵幀），跳出循環。

最後檢查 bMatch 是否為false，如果是則重定位失敗返回false；否則重定位成功，Tracking的 mnLastRelocFrameId 改為當前幀ID，返回true。