轉載請注明出處，謝謝
原創作者：Mingrui
原創鏈接：https://www.cnblogs.com/MingruiYu/p/12369339.html
本文要點：
- ORB-SLAM2 LoopClosing 線程 論文內容介紹
- ORB-SLAM2 LoopClosing 線程 代碼結構介紹
寫在前面
之前的 ORB-SLAM2 系列文章中，我們已經對 Tracking 線程和 LocalMapping 進行了介紹。我們將在本文中，對 ORB-SLAM2 系統的 LoopClosing 線程進行介紹。
依舊祭出該圖，方便查看：
也再次獻上我繪制的程序導圖全圖：ORB-SLAM2 程序導圖
老規矩，還是分兩部分：以 ORB-SLAM 論文為參考 和 以 ORB-SLAM2 代碼（程序導圖）為參考。
以 ORB-SLAM 論文為參考
LoopClosing 線程的大致步驟如下：
- 接收 LoopClosing 送來的篩選處理后的 KF
- 檢測出一批 Candidate KFs
- 計算 Sim3，確定最終的 Loop KF
- 進行回環融合
- 優化 Essential Graph
這些步驟可以歸為兩類：
- 回環檢測 Loop Dectection
- 回環校正 Loop Correction
下面我們對每一個步驟進行詳細的介紹。
接收 LoopClosing 送來的篩選處理后的 KF
在之前的 LocalMapping 線程中，會對 KFs 進行篩選，最終在 Map 中保留必要的 KFs。經過篩選后的 KFs，會送入 LoopClosing 線程，每一個新送入的 Current KF，都會檢測在 KF Database 中，有沒有以前路過的 KF，與 Current KF 相匹配，從而可以進行回環校正。
檢測出一批 Candidate KFs
這一步的目的是初步篩選出一批 Candidate KFs。
首先，會依據 KF 的 BoW 計算 Current KF 與其每一個 Covisible KF 之間的相似分數，取最低分作為 \(s_{min}\)，Candidate KFs 與 Current KF 之間的相似分數至少要大於 \(s_{min}\)（動態計算選擇 Candidate KFs 的閾值）。
另外，Current KF 的 Covisible KFs 是不能成為 Candidate KFs 的。
再之后，還要進行連續性檢測,進一步篩選 Candidate KFs：該 Candidate KF 需要在 Covisibility Graph 中與三個以上 Candidate KFs 以 Group 形式相連。
(以上部分論文中說的比較含糊，看代碼會相對清晰，下文會詳述)
計算 Sim3，確定最終的 Loop KF
在回環校正的時候，我們需要對 Current KF 和 Loop KF 之間的相對誤差進行描述。但是注意，在單目 SLAM 中，是有7個自由度的：6 + 尺度。也就是說，對於單目 SLAM，在運行過程中的累計漂移，除了平移漂移，旋轉漂移之外，還會有尺度漂移。可能跟着跟着軌跡和 Map 的尺度都整體縮小了。如果僅計算 SE3 的話，沒法很好的對回環進行校正，所以提出了 Sim3（相似變換群）：
\(\operatorname{sim}(3)=\left\{\left[\begin{array}{cc} {S=} & {s \boldsymbol{R}} & {t} \\ {\mathbf{0}^{T}} & {1} \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\}\)
其中 \(S\) 就是相似變換，其在歐式變換的基礎上加上了一個尺度因子 \(s\)。
ORB-SLAM 會計算 Current KF 與 Candidiate KF 之間的相似變換，並據此確定最終的 Loop KF，其步驟如下：
- 對 Current KF 與 Candidate KF 的 FeaturePoints 進行 BoW 匹配（要求是與 MapPoints 鏈接的 FeaturePoints），從而的到 MapPoints 與 MapPoints 之間的匹配（3D - 3D）。
- RANSAC 迭代計算每一個 Candidate KF 與 Current KF 之間的相似變換。
- 當找到一個有足夠多 inliers 的相似變換后，進行優化；根據優化得到的相似變換，可以找到更多的 Current KF 與 該 Candidate KF 的 3D - 3D 匹配。
- 再進行優化，如果又有足夠多 inliers，則認為該相似變換滿足要求，也就找到一個滿足要求的 Loop KF。
至此，就為 Current KF 確定了一個 Loop KF，Loop Detection 部分就完成了，之后就進入 Loop Correction。
回環融合
回環融合分以下幾步：
- 校正 Current KF 及其 Convisible KFs 的 SE3 位姿
- 根據 Loop KF 的位姿 和 Loop KF 與 Current KF 之間的相似變換，對 Current KF 的位姿進行校正。
- 同時，也對 Current KF 的 Convisible KFs 的位姿進行校正（在 Current KF 的校正位姿基礎上，疊加 Current KF 與 Convisible KFs 之間的相對位姿）。
- 將 Loop KF 及其 Convisible KFs 與 Current KF 及其 Convisible KFs 進行 MapPoints 融合
- 將 Loop KF 及其 Convisible KFs 所含的 MapPoints 與 Current KF 及其 Convisible KFs 的 FeaturePoints 進行匹配（通過投影找匹配），匹配上了就進行 MapPoints 融合（此處的融合和 LocalMapping 中的 MapPoinits 融合采取同樣方式）。
- 更新 Convisibility Graph
- 相關 KFs 更新其 Convisible KFs
- 添加回環邊
優化 Essential Graph
上述回環校正只對 Current KF 附近的 KFs 進行了校正，但是累計誤差是慢慢積累的，一路上每個 KF 都應該接受校正。
ORB-SLAM 采用了對 Essential Graph 進行位姿圖優化的方式。該優化是 Sim3 優化，以便校正尺度漂移。
以 ORB-SLAM2 代碼（程序導圖）為參考
請點擊 ORB-SLAM2 程序導圖鏈接（文首）查看清晰全圖
與 LocalMapping 線程一樣，LoopClosing 線程干的第一件事，也是檢查新 KF 隊列中有沒有未處理的 KF，有的話，取出隊首元素作為 Current KF。
回環檢測 —— 檢測 Candidate KFs
LoopClosing::DetectLoop()，首先，之前剛進行完回環檢測的10幀 KFs 內，不進行回環檢測，且這10幀 KFs 不會添加進 KF Database。
BoW 檢索
首先，計算 \(s_{min}\)。
之后，KeyFrameDatabase::DetectLoopCandidates() 篩選，除了論文中提到的根據 \(s_{min}\) 篩選，還要求 Candidate KF 與 Current KF 的共同 word 數 > 0.8 * 任意 KF 與 Current KF 的最多共同 word 數（但不包括 Current KF 的 Covisible KFs）。
之后，還要再篩選一次：對上述所有符合要求的 KFs，還要將它和其所有 Covisible KFs 組成一組，計算該組的相似分數之和，並且找出該組內相似分數最高的 KF。將所有 (總分數 > 0.75 * 最高組分數) 的組的最高分 KF 選入 Candidate KFs，送入下一步（這一步的意義是，如果單蹦出一 KF 相似分數很高，但它附近的 KFs 的相似分數都很低，那么可能這 KF 分高是不可靠的，需要篩掉）。
連續性檢測
進一步篩選 Candidate KFs：進行連續性檢測，該 Candidate KF 需要在 Covisibility Graph 中與三個以上 Candidate KFs 以 Group 形式相連。
什么是以 Group 形式相連呢？假設 KF1 及其 Covisible KFs 組成 Group1，KF2 及其 Covisible KFs 組成 Group2；如果 Group1 與 Group2 含有相同的 KF，則兩個 Group 相連。
所以如果一個 Candidate KF 的 Group 與 另外 3 個 Candidate KFs 的 Groups 相連，則通過篩選，進入下一環節。
回環檢測 —— 計算 Sim3，確定最終的 Loop KF
LoopClosing::ComputeSim3()，上一步中，會得到好幾個 Candidate KF，這一步的目的之一是的到最終的 Loop KF，另外，還要計算的到 Current KF 與 Loop KF 之間的相似變換。
注： 從上述回環檢測的步驟可以看出，ORB-SLAM2 在檢測回環時，分層地進行了大量的篩選步驟。這樣，一是為了保證 Loop KF 的准確性，二是避免出現大量相似回環：一般來說，因為場景是連續的，所以出現回環的地方，一般會出現一連串差不多的回環，這些篩選的步驟避免了添加重復的回環，也是減輕 LoopClosing 線程的負擔。
回環校正
LoopClosing::CorrectLoop()，首先讓通知 LocalMapping 暫停，防止插入新的 KF，並等待 LocalMapping 線程停止。期間還要停止正在進行的 Global BA（如果有的話）。
回環融合
與論文中描述的一致。
注意其中 MapPoints 的融合：將 Loop KF 所含的 MapPoints 與 Current KF 的 FeaturePoints 進行匹配，匹配上的話就將該 MapPoint 與 那個 FeaturePoint 鏈接上，但如果 Current KF 的該 FeaturePoint 已經鏈接上了某個 MapPoint，則用 Loop KF 的 MapPoint 替換掉原來鏈接的 MapPoint。
之后，再將 Loop KF 的 Covisible KFs 與 Current KF 的 Covisible KFs 兩部分的 MapPoints 做匹配，使用 ORBmatcher::Fuse()。
最后，更新這局部的 Convisibility Graph。
優化 Essential Graph
Optimizer::OptimizeEssentialGraph()
注意，這之后的 Loop KF -> AddLoopEdge(Current KF) 和 Current KF -> AddLoopEdge(Loop KF) 只是記錄回環邊。Convisibility Graph 在之前已經更新過了。
Global BA
上述步驟全部進行完了之后，精度已經很高了，但 ORB-SLAM2 還是選擇在最后再進行一次 Global BA 錦上添花（ORB-SLAM1 中似乎沒有這步）。注意，為了不影響主要3個線程的工作，這里創建了第4個線程，專門進行 Global BA。但該 Global BA 隨時可能被打斷，只有在系統特別閑的時候才會運行。