轉載請注明出處，謝謝
原創作者：Mingrui
原創鏈接：https://www.cnblogs.com/MingruiYu/p/12352960.html
本文要點：
- ORB-SLAM2 Tracking 線程 論文內容介紹
- ORB-SLAM2 Tracking 線程 代碼結構介紹
寫在前面
上一篇文章中我們已經對 ORB-SLAM2 系統有了一個概覽性的了解。通過我繪制的詳細的思維導圖形式的程序導圖，我們也可以很清晰地看出各個線程之間的關系，以及它們是如何和論文中的 System Overview 圖對應上的。
依舊祭出該圖，方便查看：
也再次獻上我繪制的程序導圖全圖：ORB-SLAM2 程序導圖
從這篇文章開始，我們將會進入 ORB-SLAM2 的每個部分，學習 ORB-SLAM2 每個部分的具體結構和邏輯。Tracking 線程是 ORB-SLAM2 系統的主線程，每一幀圖像送入后也會先經過 Tracking 線程的處理。所以這篇文章，我們先來看看 Tracking 線程的具體工作。
老規矩，還是分兩部分：以 ORB-SLAM 論文為參考 和 以 ORB-SLAM2 代碼（程序導圖）為參考。
以 ORB-SLAM 論文為參考
首先，來看看論文中對 Tracking 線程的介紹。
Tracking 線程的主要工作如下：
- 對於新讀取的幀，提取 ORB 特征
- （系統初始化）
- 當前幀位姿初值估計（根據上一幀 + motion-only BA，或進行重定位）
- 局部地圖跟蹤
- 對上一步得到的位姿初值進行進一步 BA 優化
- 局部地圖：指 Covisibility Graph 中附近的 KFs 及其 MapPoints 所組成的局部的地圖
- 決定是否將當前幀作為關鍵幀插入 LocalMapping 線程
下面我們具體來看這些內容。
注： Tracking 線程中很重要的一個工作是進行單目初始化，這一部分我會單獨寫一片文章來進行介紹，所以本文會暫時跳過單目初始化的具體內容。
ORB 特征提取
ORB-SLAM2 系統采用 ORB 特征作為貫穿整個系統使用的特征提取和描述方式。其優勢在於，提取速度快（大幅快於 SIFT 和 SURF，但其實 ORB 特征的提取還是整個系統中最耗時的部分）。關於 ORB 特征的詳細內容可見論文：ORB: An efficient alternative to SIFT or SURF PDF。
ORB 特征具有旋轉不變性，但沒有尺度不變性。為了減小尺度變化對於 ORB 特征的影響，ORB-SLAM 采用尺度金字塔的方式，將圖像放大或縮小形成不同尺度（共8個，每個尺度之間的縮放比例為1.2），之后再在每個尺度的圖像上都提取一遍 ORB 特征（提出 ORB 特征會帶有一個標記，標記其是從哪個尺度提取出來的），將每個尺度提取出的 ORB 特征匯總在一起，就成為了該圖像提取的 ORB 特征。
為了盡可能使得 提取的 ORB 特征在圖像上分布均勻（ORB 特征提取本身存在一個問題，其在圖像上分布不均，經常有的局部一大堆特征點，有的局部沒有特征點），ORB-SLAM 將每個尺度的圖像，划分成一個個小格格（切蛋糕了），在每個小格格上提取至少5個特征點。如果提取不出5個特征點，就將提取特征的閾值放低一些。
提取的 ORB 特征在 ORB-SLAM 系統中相當重要，會貫穿整個系統，用於所有的特征匹配。
當前幀位姿初值估計
Tracking 線程的目的之一是求出當前幀的位姿，其巧妙地將這個求解過程分為兩步，從粗到細。相對較粗的步驟 —— 當前幀位姿初值估計，在估計好一個初值后，會進入相對較細的步驟 —— 局部地圖跟蹤，然后得到一個最終的位姿（當然在 LocalMapping 線程中還要繼續優化）。
首先來看這個相對較粗的步驟 —— 相機位姿初值估計。其有三種可能的估計方式，論文里提到了兩種：根據上一幀和運動模型進行估計（上一幀跟蹤成功） 和 通過全局重定位估計（上一幀跟蹤丟失）。還有一種是根據 Reference KF 進行估計（雖然上一幀跟蹤成功，但因為種種原因，無法使用上一幀和運動模型進行估計），我們會在代碼部分對其進行介紹。另外，在這一部分中，除了會估計當前幀的位姿外，還會將當前幀的 FeaturePoints 和 MapPoints 做一個初步的匹配。
根據上一幀和運動模型進行估計
如果上一幀跟蹤成功，就可以繼續正常的跟蹤。ORB-SLAM 系統假設了一個勻速運動模型，意思就是假設當前幀與上一幀之間的相對位姿變化量 = 上一幀和上上幀之間的相對位姿變化量。通過這個可以先估計出當前幀的一個位姿初值，根據這個位姿初值，將上一幀的 MapPoints 和當前幀的 FeaturePoints 進行匹配，之后根據匹配進行優化。（如果沒有找到足夠多的匹配，就要使用上面提到的 根據 Reference KF 進行估計的方法了）
重定位
如果上一幀跟蹤失敗了，沒有上一幀的位姿，肯定是無法通過上面的方法繼續跟蹤的，所以要進行重定位。重定位的含義就是從 KF Database 中尋找有沒有哪個 KF 與當前幀很相似，有的話可能當前幀就在那個 KF 附近，從而定位了當前幀。
尋找可能的 KF 並計算當前幀位姿的方法如下：根據當前幀的 FeaturePoints 計算當前幀的 BoW。通過當前幀的 BoW 與 KF Database 中的 KFs 的 BoW 進行匹配，初步篩選初一批 Candidate KFs，並將當前幀的 FeaturePoints 與 Candidate KFs 含有的 MapPoints 進行匹配。之后，RANSAC 迭代計算當前幀的位姿（通過 PnP 算法求解）。注意，上述計算的目的之一是進一步篩選 Candidate KFs，所以根據每一個 Candidate KFs 都要計算出一個當前幀的位姿，直到找到一個合適的 Candidate KF，根據它計算出的當前幀位姿很合適（有足夠多的 inliers）。再對其進行進一步優化，再進行更多的 FeaturePoints 和 MapPoints 的匹配。如果再優化后這個位姿計算還很合適（有足夠多的 inliers），那就確定當前幀的位姿了，之后就可以繼續正常跟蹤了。
局部地圖跟蹤
當在上一步中獲得了當前幀的位姿初值並且當前幀的 FeaturePoints 和 MapPoints 有了初步的匹配后，就會進入這個更精細的求解當前幀位姿的步驟 —— 局部地圖跟蹤。
局部地圖里包括：
- 和當前幀有共同觀察到的 MapPoints 的 KFs
*上述 KFs 的 Covisible KFs - 它們含有的某些 MapPoints
- 該 MapPoint 可以投影到當前幀的畫幅內
- 該 MapPoint 的平均可視方向與當前幀的方向的夾角不大於60度
- 該 MapPoint 距離當前幀光心的距離在一定范圍內（范圍太大的話，ORB 特征的尺度不變性很難保證，這樣匹配出錯的概率很大）
在計算得到局部地圖的同時，還需要盡可能進一步地將局部地圖的 MapPoints 與 當前幀還未匹配的 FeaturePoints 相匹配。最終，對該局部地圖進行 BA 優化。
決定是否將當前幀作為 KeyFrame
如果當前幀比較重要，則會將其作為 KF 插入 Map 並送入 LocalMapping 線程。ORB-SLAM 的一個特點就是，其插入 KF 的條件很寬松，這樣會插入很多 KFs，而 ORB-SLAM 會在 LocalMapping 線程中對它們中冗余的進行剔除。這樣的目的是不放過可能有用的幀，增強系統的魯棒性，以應對純旋轉等很難處理的相機運動。
雖然這個條件很寬松，但還是有條件的：
- 距離上一次重定位已經過去了超過20幀
- LocalMapping 線程正閑置，但如果已經有連續20幀內沒有插入過 KF 了，那么 LocalMapping 線程不管忙不忙，都要插入 KF 了 （忙就給老子停下hhh）
- 當前幀至少追蹤了 50 個點（當前幀質量不能太差）
- 當前幀追蹤的點中不能有90%以上和其 Reference KF 相同（當前幀不能太冗余）
以 ORB-SLAM2 代碼（程序導圖）為參考
看完了論文，可能有點不好理解，畢竟用文字進行描述的難度是很高的，特別是 ORB-SLAM2 系統內部各部分之間的邏輯又是較為復雜的。Talk is cheap, give me code. 下面我們從 ORB-SLAM2 的代碼出發，結合我繪制的 ORB-SLAM2 程序導圖，進一步加深對 Tracking 線程的理解。
如上圖所示，在 System.cc 的主循環中，啟動了對於 Tracking 線程的調用。每次讀入一幀圖像，為其創建 Frame 對象，在創建的同時就提起了 ORB 特征。這里有一個細節，在單目初始化時，對於該幀提取的 ORB 特征數是平時的兩倍。
如上圖所示，進入 Tracking 線程中，可以看到一個很重要的狀態變量為 mState，根據它來區分當前系統的狀態。當系統還未初始化時，會運行 MonocularInitialization() 來進行初始化。關於這一部分會在后面的博文中專門介紹。
初始化之后，就會進入常規 Tracking 過程，其中首先進行當前幀位姿，之后進行局部地圖跟蹤，再之后決定是否生成 KF，並插入 KF。下面我們一個部分一個部分來看。
因為這些地方實在不好截圖，請大家點擊這張導圖的鏈接 （在文首）來查看清晰大圖吧。
注：
ORB-SLAM2 系統有兩種模式（可以由使用者手動切換），其以 mbOnlyTracking 變量進行區分：
- SLAM 模型：所有線程都正常工作
- Localization 模式：只有 Tracking 線程工作，其它線程均不工作
同時，Localization 模式中也有兩種情況（系統自動判定，根據當前跟蹤情況自動切換），其以 mbVO 變量進行區分：
- VO 情況：Visual Odometry，上一幀追蹤到的點大部分是 VO 點（未能與 MapPoints 匹配（此處存疑）），此時不會進入局部地圖跟蹤，直到重定位成功，具體后面細說。
- 正常情況：常規，所有部分正常運行。
當前幀位姿初值估計
如果是 SLAM 模式，則首先根據 mState 判斷系統之前的跟蹤狀態。如果之前跟蹤丟失，則要不斷進行重定位 Tracking::Relocalization()，直到當前幀與 KF Database 中的某個 KF 匹配上了。如果之前跟蹤正常，則繼續跟蹤，一般來說使用 Tracking::TrackWithMotionMode() 進行估計，但如果運動模型還未建立，或者剛剛進行了重定位，則使用 Tracking::TrackReferenceKeyFrame() 進行估計。TrackReferenceKeyFrame() 指當前幀和其 Reference KF 進行匹配來估計位姿，其匹配的搜索量會大很多，所以當 Tracking::TrackWithMotionMode() 不行的時候才會用它。
具體的位姿估計方式都是 匹配 + 優化。只是匹配的方式會有所不同：
- Tracking::TrackReferenceKeyFrame() 是根據 BoW 來在當前幀所有提取出的 FeaturePoints 和 Reference Frame 的 MapPoints 中進行匹配（當然使用 BoW 有可以減少計算量的方法）；
- Tracking::TrackWithMotionMode() 中是有了位姿初值，所以可以根據該初值進行投影，將上一幀的 MapPoints 先投影至當前幀的一個大概區域，從而縮小了搜索的區域大小，減小了搜索量。
如果是 Localization 模式，那么如果之前系統跟蹤丟失，同樣不斷進行重定位 Tracking::Relocalization()。如果之前系統跟蹤正常，與 SLAM 模式不同的地方在於，其會判斷當前處於 VO 情況還是正常情況：
- 正常情況：與 SLAM 模式基本一致，根據運動模式是否已經建立而采用 Tracking::TrackWithMotionMode() 或 Tracking::Relocalization()。
- VO 情況：與 SLAM 模式不一樣了，此時進行 Tracking::TrackWithMotionMode() 和 Tracking::Relocalization()，優先使用 Relocalization() 的結果（此時重定位的結果更可靠一些），如果重定位失敗，則采用 Tracking::TrackWithMotionMode() 繼續跟蹤甚至直接丟失，如果重定位成功，則可以推出 VO 模型回到正常模式。
局部地圖跟蹤
只有 SLAM 模式下，且上一步當前幀位姿初值估計成功（有位姿初值了）的情況下才會進行局部地圖跟蹤。
在局部地圖跟蹤優化后，會判斷優化的效果如何，如果效果可以的話，才會判斷本次跟蹤成功（當前幀位姿初值估計 + 局部地圖跟蹤 都成功才算成功），否則本次跟蹤丟失。
決定是否生成關鍵幀，並插入關鍵幀
如果當前幀丟失的話，那肯定是不會將其作為 KF 插入的。但剛初始化完沒幾幀就丟失了，說明初始化的質量不行，系統 Reset，重新初始化。（從中可以看出，ORB-SLAM2 對於初始化的質量標准很高，所以也經常出現在實際中其遲遲不肯啟動的狀況）。
如果當前幀跟蹤成功，更新運動模型，且根據論文中的標准決定當前幀是否作為 KF 插入 Map，並送入 LocalMapping 線程。