轉載請注明出處，謝謝
原創作者：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 了。