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

時間 2020-02-24

標籤 orb slam2 slam 論文代碼學習 tracking 線程简体版

原文原文鏈接

轉載請註明出處，謝謝
原創做者：MingruiYu
原創連接：http://www.javashuo.com/article/p-amuyogdu-hb.htmlhtml

本文要點：算法

ORB-SLAM2 Tracking 線程論文內容介紹
ORB-SLAM2 Tracking 線程代碼結構介紹

寫在前面

上一篇文章中咱們已經對 ORB-SLAM2 系統有了一個概覽性的瞭解。經過我繪製的詳細的思惟導圖形式的程序導圖，咱們也能夠很清晰地看出各個線程之間的關係，以及它們是如何和論文中的 System Overview 圖對應上的。app

依舊祭出該圖，方便查看：dom

也再次獻上我繪製的程序導圖全圖：ORB-SLAM2 程序導圖學習

從這篇文章開始，咱們將會進入 ORB-SLAM2 的每一個部分，學習 ORB-SLAM2 每一個部分的具體結構和邏輯。Tracking 線程是 ORB-SLAM2 系統的主線程，每一幀圖像送入後也會先通過 Tracking 線程的處理。因此這篇文章，咱們先來看看 Tracking 線程的具體工做。優化

老規矩，仍是分兩部分：以 ORB-SLAM 論文爲參考和以 ORB-SLAM2 代碼（程序導圖）爲參考。ui

以 ORB-SLAM 論文爲參考

首先，來看看論文中對 Tracking 線程的介紹。.net

Tracking 線程的主要工做以下：線程

對於新讀取的幀，提取 ORB 特徵
（系統初始化）
當前幀位姿初值估計（根據上一幀 + motion-only BA，或進行重定位）
局部地圖跟蹤
- 對上一步獲得的位姿初值進行進一步 BA 優化
- 局部地圖：指 Covisibility Graph 中附近的 KFs 及其 MapPoints 所組成的局部的地圖
決定是否將當前幀做爲關鍵幀插入 LocalMapping 線程

下面咱們具體來看這些內容。code

注： 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 模型回到正常模式。