前言
距離上一次博客更新已經過去了N年。。。
ORB-SLAM2繼續更新，這真的是有生之年系列。
ORB-SLAM3在2的基礎上，引進了IMU的初始化和融合估計，以及子地圖功能。筆者目前 IMU 相關的知識相對比較薄弱，因此准備在第十四講的時候再為大家介紹VI-ORBSLAM相關的內容。本講主要關心的是純視覺的子地圖融合功能。
ORB-SLAM子地圖簡介
ORB-SLAM3使用子地圖的原因是：當我們在跟蹤丟失的情況下，不至於在唯一的一張地圖中瘋狂重定位。因為重定位的結果只有兩個，一個是馬上重定位成功，那么這是最理想的情況，我們可以繼續進行環境探索；另一種就比較吃虧了，我們一直無法重定位成功，最終的結果就是系統判斷當前跟蹤狀態為跟蹤丟失，系統會直接將之前所重建的環境信息全部清除！重新生成一張地圖。
這意味着什么？這就好比我們辛辛苦苦在word里面寫了成千上萬字的文檔，word一個bug直接把你所有的數據清空了，你還找不回，只能從頭寫那么頭疼！
那么解決的策略是啥？ORB-SLAM3就提出用子地圖的方案！那么子地圖好在哪里？我們還是以word為例：
當我們寫了成千上萬字的word文檔被bug卡沒以后，這部分的文字會自動存儲到某個位置（歷史子地圖），新增的文字會寫在另一個word文檔中（當前子地圖），當緣分來臨的時候（回環檢測），這兩個word文檔就有機會合並到一起（子地圖合並），形成統一的一個word文檔！oh man，想想就覺得激動人心。
ps：筆者無意針對word文檔，只是因為前不久有個同事剛好因為word把文檔內容卡沒了，所以筆者才以此為例，xxxxxxx。。。
ORB-SLAM子地圖算法
在介紹子地圖功能之前，我們有幾個問題需要着重關心一下：
1. 子地圖功能和ORB-SLAM2中使用的單一地圖的方法有什么異同？
2. 存在多個子地圖的情況下，回環檢測怎么做？這個方法與ORB-SLAM2中的回環檢測有何區別？
3. 兩個子地圖怎么進行合並？
4. 合並的過程中，前端tracking到的新幀要怎么處理，此處必然會存在一個異步問題。
OK，那么我們逐個問題來回答！
1. 子地圖的好處
其實子地圖的好處我們已經在簡介的時候有所提及，當我們只維護一個地圖，在跟蹤丟失的情況下會進行重定位。此時，重定位有兩種情況：一種是重定位失敗，地圖重置；另一種是重定位成功，但是重定位成功也可能會出現兩種情況：第一種是估算的相對變換准確，使得我們的機器人跟蹤回到正確的道路上，這當然是最理想的情況！但是還有另一種情況會非常致命，那就是估算的相對變換出現偏差，導致最終的地圖出現嚴重的不一致性。論文“ORBSLAM-Atlas: a robust and accurate multi-map system”中就介紹了重定位估算pose出現偏差的情況：
可見，上述左圖即是ORB-SLAM2中只維護一個地圖會出現的情況。而上述右圖則是采用子地圖的策略得到的結果，孰優孰劣一目了然。
除此之外，筆者認為子地圖策略還有一個好處：那就是可以盡可能保證不丟幀，畢竟ORB-SLAM2需要進行多次重定位失敗才會最終進行地圖重置，而ORB-SLAM3只要一定位失敗則直接重新創建子地圖，可以有效地保存盡可能多的圖像，這對於幀率較低的應用場景非常友好！
2. 回環檢測
ORB-SLAM3的回環檢測方法相比於2的有所改進，首先確定的一個地方是：即使系統中存在子地圖，我們也只會存儲唯一的一個BOW數據集，而每個關鍵幀都會存儲各自所在的子地圖，以此來加以區分。
具體的回環檢測策略大體上與ORB-SLAM2並無太大的區別，即：
a. 將tracking階段判斷為關鍵幀的幀插入回環檢測線程中，轉換為BOW向量，在唯一的BOW數據集中搜索最相似的前幾個候選關鍵幀，這與我們之前介紹的（八）ORBSLAM回環檢測之位置識別 並無不同。
b. 對每個候選關鍵幀進行逐一篩查，具體為搜索幀間匹配，基於匹配關系確定對應的地圖點之間的關系，從而計算相對變換，進一步驗證投影匹配點數量是否滿足要求。
最大的不同之處在於ORB3不需要在時序上有至少連續三幀被檢測為回環，而是：
c. 與當前關鍵幀共視區域最多的 5 個關鍵幀，回環候選幀與其中至少 3 個關鍵幀存在足夠的共視關系，即投影匹配滿足約束要求，則視為回環。
這種方法相比於ORB-SLAM2的時序性要求來說，可以有效地提高召回率（在所有真實回環中被正確檢測出來的概率），使得系統在該回環的時候回環，可以有效提高整體的精度。
最后一個值得注意的地方是，ORB3中存在多個子地圖，在檢測到最終的回環幀后，我們還需要判斷當前回環的位置：
d. 判斷回環幀所在的子地圖，如果回環幀是在當前激活的子地圖當中，那么則在當前子地圖中執行閉環操作；如果回環幀是在歷史子地圖當中，那么則將當前子地圖和歷史子地圖進行融合，並以歷史子地圖為主，將當前子地圖融合到歷史子地圖中，並將當前子地圖剔除，將融合后的歷史子地圖設置為當前子地圖。
此處需要特別特別注意的是，由於基於詞袋模型進行檢索，可能會同時出現既有當前地圖的回環候選幀，也有歷史地圖的回環候選幀．
3. 子地圖融合
如果回環幀是在當前子地圖中，則執行閉環操作，具體思路如：（十）ORBSLAM閉環所述。
如果回環幀是在歷史子地圖中，則執行子地圖融合操作，具體思路如下：
由於在看代碼的時候做了腦圖，因此筆者也就偷個懶，直接把部分圖摳出來放這里了．
合並兩個子地圖的思路非常直接，跟回環的閉環操作非常相似：
a. 當前幀與回環幀的子地圖構建匹配關系，首先是基於ＢＯＷ搜索匹配；
b. 滿足匹配約束以后計算回環幀的共視圖與當前幀的匹配關系，從而找到更多的匹配關系；
c. 根據匹配關系估計一個相對變換關系；
d. 根據相對變換關系將回環幀子地圖投影到當前幀中去搜索更多匹配關系，進而優化相對變換關系；
e. 將這個相對變換關系作為兩個地圖之間的＂紐帶＂，即一條強約束的邊，再對兩個共視窗口進行 Local Bundle Adjustment，但是會先固定回環幀的局部地圖，只調整當前幀的局部地圖；
f. 基於LBA的結果，將當前幀的局部地圖全部轉換到回環幀的子地圖中，並將其從當前地圖刪除；
g. 最后將當前地圖中的剩余幀全部轉換到回環幀子地圖中．
恩...思路確實是非常朴素的，不過在使用策略上需要有一些要注意的地方，這些問題才是實際應用當中需要注意，比如：
a. 子地圖什么時候才穩定下來，應該要保證每個子地圖至少多少幀才用起來？
b. 在融合子地圖之后，合並出來的結果會不會對之前導出的結果什么影響？
c. 理論上，所有子地圖最好有一個標准的基准，比如一個世界坐標系，然而純視覺的每一次初始化都定義不同的世界坐標系，那該怎么辦？
4. 線程異步問題
這個問題應該是挺多人在讀代碼的時候比較容易忽略的地方，然而在實際功能添加的時候，這個卻是沒有辦法忽略的．
之前和范帝楷范大佬探討過這個問題，這次也順便在這里提一下．
首先需要明確的是，ORB-SLAM3一共有３個主線程，還有一個是隨時需要隨時啟動的GBA線程，即共計４個線程．
a. 在進行子地圖合並的時候，首先關閉的功能就是GBA，如果GBA還有任務在進行，就強行將其終止，並且釋放掉該線程．OK，GBA這個線程的異步問題我們可以直接忽略了；
b. 那么tracking這個線程咋辦？這個線程我們可沒有辦法終止它，一旦終止了就意味着不再跟蹤了，顯然對於我們的實時定位肯定會存在大問題，因此這個地方我們不動他，可是他會一直產生新的關鍵幀，咋辦？
c. 既然tracking沒法關，Local Mapping總可以了吧？在關完GBA以后，我們就把LocalMapping也給關了，並且把所有新關鍵幀直接插入當前子地圖中，不做額外處理，全部到合並子地圖環節進行操作．這里有兩個地方需要注意：
c1. 在進行當前幀共視圖和回環幀共視圖之間的融合時，Local Mapping是始終關閉的，因此tracking關鍵幀插不進來；
c2. 在c1步驟的融合結束以后，Local Mapping線程會被釋放，因此此時新的關鍵幀會正常在Local Mapping中進行插入，此時調用當前子地圖可以得到所有新插入的關鍵幀，從而將其進行轉換，進而將新幀也一並融合了．
d. 最后這里可能還需要做一個pose graph的優化，此處是對當前所有關鍵幀的Pose進行聯合優化，也是需要鎖定Local Mapping的．
總結：
按照慣例，我們還是需要進行總結！
這一講我們主要介紹了ORB-SLAM3中引入的子地圖功能，其中包括其與回環檢測操作的異同，以及子地圖的關鍵方法和工程實現上可能存在坑的地方．
當然，筆者也是認知有限，難免有所偏差，還請各位讀者多多批評指正．
后續應該還會有第十四講，筆者需要去補補IMU等相關的知識，以期給大家帶來VI-ORB相關的介紹，防止誤人子弟．
還望各位讀者朋友們能繼續支持我！
參考文獻：
[1] ORBSLAM-Atlas: a robust and accurate multi-map system
[2] ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
PS：
如果您覺得我的博客對您有所幫助，歡迎關注我的博客。此外，歡迎轉載我的文章，但請注明出處鏈接。
對本文有任何問題可以在留言區進行評論，也可以在泡泡機器人論壇：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技術交流模塊發帖提問。
我的github鏈接是：https://github.com/yepeichu123/orbslam2_learn。