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

轉載請註明出處，謝謝
原創做者：Mingrui
原創連接：http://www.javashuo.com/article/p-erxcszco-dx.html

---

本文要點：

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

寫在前面

以前的 ORB-SLAM2 系列文章中，咱們已經對 Tracking 線程和其中的單目初始化部分進行了介紹。咱們將在本文中，對 ORB-SLAM2 系統的 LocalMapping 線程進行介紹。

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

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

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

以 ORB-SLAM 論文爲參考

LocalMapping 線程的大體步驟以下：

• 接收從 Tracking 線程插入的 KF，並進行預處理
• 剔除質量較差的 MapPoints
• 經過三角化生成新的 MapPoints
  • Current KF 未與現有 MapPoints 匹配的 FeaturePoints 與其 Covisible KFs 的 FeaturePoints 進行匹配，並三角化
• Local BA
• 剔除冗餘的局部 KF

LocalMapping 線程的存在主要有這麼幾個意義：

• 篩選 KFs
• 進一步優化 Tracking 線程獲得的 KFs 位姿以及 MapPoints 座標，但這個優化仍是相對輕量級的（與 LoopClosing 線程相比），且這裏的優化不涉及迴環

下面咱們對每個步驟進行詳細的介紹。

插入 KF

當 Tracking 線程肯定一個要插入的 KF 時，實際上它並無真的完成將 KF 插入 Map 的動做。當咱們將一個 KF 插入 Map 中時，咱們須要同時作不少更新工做：

• 更新 Covisibility Graph（在 Covisibility Graph 中添加新的 KF node，根據共視關係添加新的 edge）
• 更新生成樹
• 計算新 KF 的 BoW （便於後面經過特徵匹配和三角化生成新的 MapPoints）

剔除質量較差的 MapPoints

存儲在 Map 中的 MapPoints 須要有較高的質量（追蹤良好，三角化正確），因此此處須要採起一些措施去掉質量較差的 MapPoints。判斷 MapPoints 質量較差的標準爲，在該 MapPoint 被創造後的3個 KFs 時間範圍內：

• 實際看到該 MapPoints 的幀數 / 應該看到該 MapPoints 的幀數 < 25% （注意不僅是 KFs）
  • 應該看到該 MapPoints 的幀：當咱們有了某 MapPoint，也有了某幀的位姿時，咱們能夠經過投影判斷該 MapPoint 是否在該幀的視野內，這些幀就是應該看到該 MapPoints 的幀
  • 實際看到該 MapPoints 的幀：經過各類匹配方式，該 MapPoints 與某幀的某個 FeaturePoint 匹配上了，這些幀就是實際看到該 MapPoints 的幀
• 該 MapPoints 在被創造後，未能被至少3個 KFs 觀測到（此處論文表達彷佛不太清楚）

注意：即便 MapPoints 在創造知足上述要求，得以保留，但不表明它們之後不可能被剔除。若是以後由於 KF 的剔除（下文會講）致使觀測到該 MapPoint 的 KF 數少於3個，或者在 local BA 中該觀測被認爲是 outlier，那麼它依然會被剔除。

經過三角化生成生成新的 MapPoints

對於單目 ORB-SLAM 來講，整個系統中只有兩處能夠在 Map 中添加 MapPoints：一處是初始化的時候，另外一處就是這裏。

ORB-SLAM 將在 Current KF 的未能與已存在 MapPoints 匹配上的 FeaturePoints，與其 Covisible KFs 中一樣未能與已存在 MapPoints 匹配上的 FeaturePoints 進行匹配。若是匹配上了，則能夠經過三角化，生成一個新的 MapPoint（生成以後要檢查其位置，視差，重投影偏差，尺度一致性）。這個 FeaturePoint 與 FeaturePoint 之間的匹配是經過 BoW 搜索實現的。

經過兩個 KFs 生成新的 MapPoint 後，還要檢查它有沒有在別的 KFs 中出現。因此要將該 MapPoint 投影至其餘的 Covisible KFs，與它們的 FeaturePoints 進行匹配。匹配上的話就將該 MapPoint 與那個 KF 的那個 FeaturePoint 連接上。

局部 BA

對 Current KF 及其 Covisible KFs 及其它們所觀察到的全部 MapPoints 進行 BA 優化。

注意，其餘觀測到這些 MapPoints，可是再也不上述 KFs 之列的 KFs，也會做爲約束參與該優化（其自己不會被優化）。

剔除冗餘的局部 KF

在 Tracking 線程中，ORB-SLAM 以很是寬鬆的條件，向 Map 中插入了不少不少 KFs，但顯然 Map 中是不能永久保留這麼多 KFs 的，這會使 Map 過於龐大，且極大增長各類 BA 的運算量。因此要剔除一些信息冗餘的。

若是 Current KF 及其 Covisible KFs 中，有哪一個 KF 它所觀測到的 90% 的 MapPoints 都能被其它至少3個（尺度相同或更好的）KFs 觀測到，則這個 KF 的信息就算做是冗餘的，就把它去掉。這樣作的目的是讓 Map 的 KF 數不要太多，且在規模必定的場景內，Map 中的 KF 數目不要無上限的增加。這樣也有利於減輕 BA 優化的負擔。

以 ORB-SLAM2 代碼（程序導圖）爲參考

上圖就是 LocalMapping 線程的程序導圖，從中能夠很清晰地看出 LocalMapping 線程的邏輯，而且和論文中的步驟進行對應。

若是嫌這張圖不夠清晰的話，能夠點擊 ORB-SLAM2 程序導圖連接（文首）查看清晰全圖

插入 KF

在插入 KF 後，會經過 LocalMapping::SetAcceptKeyFrames(false) 通知 Tracking 線程，LocalMapping 線程正忙。記得在 Tracking 線程中最後一步決定是否插入關鍵幀時，有一個條件就是：

• LocalMapping 線程正閒置，但若是已經有連續20幀內沒有插入過 KF 了，那麼 LocalMapping 線程無論忙不忙，都要插入 KF 了

另外，LocalMapping 線程經過維護一個隊列來存儲 Tracking 線程送入，但還未被 LocalMapping 處理的 KFs。LocalMapping::CheckNewKeyFrames() 用來檢查該隊列裏有沒有 KF。

從上述隊列中取出隊首 KF，使用 LocalMapping::ProcessNewKeyFrame() 對其進行處理，包括計算該 KF 的 BoW，以及更新 Covisibility Graph。最後，通過上述處理的 KF 才能夠真正插入 Map 之中。

剔除質量較差的 MapPoints

LocalMapping::MapPointCulling()

經過三角化生成新的 MapPoints

LocalMapping::CreateNewMapPoints()

MapPoints 融合

當隊列中全部的 KFs 都通過上述處理了（隊列空了），那麼纔會開始接下來的步驟。

MapPoints 融合，這部分實際上是屬於經過三角化生成新的 MapPoints 裏的，論文中說過：「經過兩個 KFs 生成新的 MapPoint 後，還要檢查它有沒有在別的 KFs 中出現。因此要將該 MapPoint 投影至其餘的 Covisible KFs，與它們的 FeaturePoints 進行匹配。匹配上的話就將該 MapPoint 與那個 KF 的那個 FeaturePoint 連接上」，這一步的目的就在與完成這項工做。

可是，這裏須要注意，在上述表述中，「匹配上的話就將該 MapPoint 與那個 KF 的那個 FeaturePoint 連接上」，但若是這些條件都麼知足，但那個 FeaturePoint 已經連接上了某個 MapPoint 怎麼辦？ORB-SLAM 採起的策略很簡單，用新的 MapPoint 替換掉原來連接的 MapPoint。

舉一個可能出現這種狀況的情景：同時有4個剛送入 LocalMapping 線程的 KFs 觀測到了 MapPoint_1 （MapPoint_1 此前未在 Map 中建立）。在上文三角化的過程當中，假設 KF_1 和 KF_2 三角化生成了 MapPoint_1，但同時 KF_3 和 KF_4 也三角化生成了 MapPoint_1。隊列中全部 KFs 處理完畢後，此時，我在將 KF_1 的 MapPoint 投影至 KF_3 時，就會發現 KF_3 的匹配 FeaturePoint 已經連接了 MapPoint了。此時須要一個融合策略（ORB-SLAM 簡單的採用了替換的方法）。

Local BA

當隊列中全部的 KFs 都通過上述處理了（隊列空），且 其餘線程沒有讓 LocalMapping 線程暫停（後面會提到 LoopClosing 線程中有地方會讓 LocalMapping 線程中的 Local BA 先暫停），則進行 Optimizer::LocalBundleAdjustment()。

剔除冗餘的 KFs

LocalMapping::KeyFrameCulling()

最後經過 LocalMapping::SetAcceptKeyFrames(true) 通知 Tracking 線程，LocalMapping 線程閒下來了，能夠有條件的接收 KFs 了。

ORB-SLAM2 系列博文

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

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

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

ORB-SLAM2 論文&代碼學習 —— 單目初始化

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