轉載請註明出處，謝謝
原創作者：Mingrui
原創連結：https://www.cnblogs.com/MingruiYu/p/12360913.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 執行緒

相關推薦