轉載請註明出處，謝謝
原創作者：MingruiYu
原創連結：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 執行緒。

ORB-SLAM2 系列博文

ORB-SLAM2 初體驗 —— 配置安裝

ORB-SLAM2 論文&程式碼學習 —— 概覽

ORB-SLAM2 論文&程式碼學習 —— Tracking 線