ORBSLAM閉環簡介
SLAM中閉環的目的，是將當前的相機位姿和場景與回環幀處的位姿和場景進行融合，從而消除累積誤差，校正運動軌跡。那么達到這個目的的前提，是必須要檢測到正確的回環。當然，這個要求在前面我們講解回環檢測的時候已經詳細介紹過了，如果不能通過層層篩選，那么我們會選擇不要閉環。但是，在檢測到正確的回環以后，我們還需要保證閉環的融合和校正是高質量的，這樣才能消除累積誤差。ORBSLAM2也盡可能地在滿足這個要求。
接下來，我們就按下面兩個方面來介紹閉環模塊：
1. Bundle Adjustment；
2. ORBSLAM2閉環方法提綱；
Bundle Adjustment
相信很多人都知道，在SLAM最開始的時候，我們都是通過濾波的方式來做姿態優化的，像卡爾曼濾波、拓展卡爾曼濾波、信息濾波和粒子濾波等等。但是基於濾波的方法都是假設變量滿足一階馬爾可夫假設的，也就是當前時刻機器人的狀態只和前一時刻的狀態有關，再往前的都不考慮。因此，這會導致一個問題，如果每一次的估計都存在一點點誤差，隨着時間的積累，這個誤差就會越來越大，導致最終算法不可用，這對於我們來說通常是難以接受。
於是人們便想着改變這種模式，將先前的狀態全部考慮進來，從而減少甚至消除累積誤差，這也就產生了我們現在最常見的非線性優化的方法。但是這種方法存在一個非常大的問題，在我們利用迭代求解的時候，每次都需要計算一個海賽陣/其他替代矩陣的逆。對於SLAM問題來說，通常問題的規模都相對比較大，比如一個相機pose可以同時觀測到上百個地圖點，於是隨着相機的運動，地圖點的累積速度將非常快，而這個海賽陣/其他替代矩陣就會變得非常龐大，求解其逆的過程也變得更加困難。
直到后來，人們發現了SLAM問題的稀疏性，即每個小的雅可比，比如 $J_{ij}$，只在相機 $ID = i$ 與地圖點 $ID = j$ 有連接的邊的地方有非0值，其他地方均為0。通過 $J^{T}J$ 構成的矩陣滿足這樣的條件：
$J^{T}J = \begin{bmatrix}
B & E \\
E^{T} & C
\end{bmatrix}$
其中，$B$ 和 $C$ 是兩個對角塊矩陣，在矩陣求逆的時候我們只需對各個塊直接求逆即可，非常方便。而 $E$ 的稠密程度取決於觀測內容。因此，才有了我們現在用的非線性優化的工具。
講到這里，基本就可以開始介紹我們的內容了。我們比較熟悉的BA是用在struct from motion里面做三維重建，struct from motion是離線的，並且數據來源是來自不同相機不同場景不同時間的。而SLAM問題的數據，基本都是一個時間序列里同一個場景同一個相機采集的，所以SLAM面對的問題相對於struct from motion的難度稍微小一些。當然，這只是單純的從重建的角度來說，涉及其他一些方面，可能SLAM的難度就會比較大，這個我們不做詳談。
BA和我們先前介紹的運動估計方法是一致的：https://www.cnblogs.com/yepeichu/p/10746952.html
不過我們在介紹運動估計的時候並沒有討論稀疏性的問題，所以在這一講我們再做一下補充。
假設我們的待估計變量為 $x = [\xi_{1}, \dots, \xi_{m}, p_{1}, \dots, p_{n}]^{T}$，其中 $\xi_{1}, \dots, \xi_{m}$ 表示軌跡中的 $m$ 個運動狀態矩陣的李代數，而 $p_{1}, \dots, p_{n}$ 表示所有觀測點在世界坐標系中的三維空間坐標。
在BA中，我們每次通過增加 $\Delta x$，使得誤差下降一點點，多次迭代以后，誤差值達到我們的閾值要求，這個問題就算收斂了。所以關鍵的問題就是求解這個 $\Delta x$。
我們再描述一下我們的誤差函數：
\begin{equation}
\frac{1}{2}\|f(x)\|_{2}^{2} = \frac{1}{2}\sum\limits_{i=1}^{m}\sum\limits_{j=1}^{n}\|e_{ij}\|_{2}^{2} = \frac{1}{2}\sum\limits_{i=1}^{m}\sum\limits_{j=1}^{n}\|u_{j} - \frac{1}{s_{j}}Kexp(\xi_{i}^{\land})P_{j}\|_{2}^{2}
\end{equation}
這就是一個非常簡單的重投影誤差，跟我們前面介紹的內容一致。假設誤差 $e_{ij}$ 對位姿 $\xi_{i}$ 的雅可比為 $F_{ij}$，對地圖點 $p_{j}$ 的雅可比為 $E_{ij}$，則誤差函數的增量形式可以描述成下列的形式：
\begin{equation}
\frac{1}{2}\|f(x+\Delta x)\|_{2}^{2} \thickapprox \frac{1}{2}\sum\limits_{i=1}^{m}\sum\limits_{j=1}^{n}\|e_{ij} + F_{ij}\Delta\xi_{i} + E_{ij}\Delta p_{j}\|_{2}^{2}
\end{equation}
我們通過將增量函數進行一階泰勒展開近似，於是得到公式$(2)$的樣子。為了更加簡潔的描述問題，我們將三維點和位姿分成兩個部分,即：
\begin{equation}
x_{c} = [ \xi_{1}, \xi_{2}, \dots, \xi_{m}]^{T} \in R^{6m}
\end{equation}
\begin{equation}
x_{p} = [ p_{1}, p_{2}, \dots, p_{n}]^{T} \in R^{3n}
\end{equation}
那么原始變量可以描述成 $x = [x_{c}, x_{p}]$。同理，對應的雅可比按照位置合成一個更大的雅可比矩陣，將其記為 $J = [F, E]$，於是可以簡化問題為：
\begin{equation}
\frac{1}{2}\|f(x+\Delta x)\|_{2}^{2} \thickapprox \frac{1}{2}\|e + J\Delta x^{T}\|_{2}^{2} = \frac{1}{2}(e^{T}e + 2e^{T}J\Delta x + \Delta x^{T}J^{T}J\Delta x)
\end{equation}
公式 $(5)$ 對增量求導，並令導數為0，則有：
\begin{equation}
J^{T}J\Delta x = -J^{T}e \Rightarrow H\Delta x = g
\end{equation}
值得注意的是，這個 $H = J^{T}J$ 矩陣，就是我們介紹的，具有稀疏性的矩陣。求解公式 $(6)$ 的增量問題，可以利用 $H$ 矩陣的稀疏性，先求個一部分增量，然后利用求出來的這部分增量去求解其它的部分。這種方法叫Schur消元法，在SLAM里面也叫邊緣化。先將我們的問題列出來：
\begin{equation}
\begin{bmatrix} B&E\\E^{T}&C \end{bmatrix} \begin{bmatrix}\Delta x_{c}\\ \Delta x_{p} \end{bmatrix} = \begin{bmatrix}v\\w\end{bmatrix}
\end{equation}
為了先求出一個變量，我們通過高斯消元（其實就是行列式變換），將系數矩陣轉換成一個下三角矩陣：
\begin{equation}
\begin{bmatrix} I&-EC^{-1}\\0&I \end{bmatrix} \begin{bmatrix} B&E\\E^{T}&c \end{bmatrix} \begin{bmatrix} \Delta x_{c}\\ \Delta x_{p} \end{bmatrix} = \begin{bmatrix} I&-EC^{-1}\\0&I \end{bmatrix} \begin{bmatrix} v\\w \end{bmatrix}
\end{equation}
簡單地整理以后，我們就得到：
\begin{equation}
\begin{bmatrix} B-EC^{-1}E^{T}&0\\E^{T}&C \end{bmatrix} \begin{bmatrix}\Delta x_{c}\\ \Delta x_{p} \end{bmatrix} = \begin{bmatrix}v-EC^{-1}w\\w\end{bmatrix}
\end{equation}
於是我們便可以得到兩個方程，分別是：
\begin{equation}
[B-EC^{-1}E^{T}]\Delta x_{c} = v - EC^{-1}w
\end{equation}
\begin{equation}
E^{T}\Delta x_{c} + C\Delta x_{p} = w
\end{equation}
先求解出 $\Delta x_{c}$，再求出 $\Delta x_{p}$。值得注意的是，求$\Delta x_{c}$ 時，線性方程的維度與B矩陣一樣。盡管兩個式子都需要求解C矩陣的逆，但是我們在前面介紹過，B矩陣和C矩陣都是對角塊矩陣，求逆只需要對各個對角塊分別求逆就可以了，計算效率非常高。
於是，我們的BA問題，通過矩陣的稀疏性，就可以使我們的SLAM問題能夠在可預期的時間里計算出來。而不是對一個dense的大規模矩陣直接求逆，這個直接求逆的計算量是問題規模的三次方，對於大規模的SLAM問題，顯然是無法實現的。
在求解BA問題時，我們對所有的誤差項都是一視同仁的。這通常會導致一個問題，如果所有匹配都是正確的，固然是好。可是如果有錯誤匹配，這會出現一個非常大的誤差項，而優化算法為了降低誤差，會選擇遷就這個誤差項，最終導致求解的結果與我們的預期不符。所以，我們會用到魯棒核函數來控制誤差項，防止錯誤匹配的影響。
比如，最常見的Huber核（參考視覺SLAM十四講）：
\begin{equation}
H(e) = \left\{
\begin{array}{lrl}
\frac{1}{2}e^{2} && {|e|\leq \delta} \\
\delta(|e| - \frac{1}{2}\delta) && {else}
\end{array}\right.
\end{equation}
當誤差項大於閾值時，函數增長由二次形式變成了一次形式，限制了錯誤匹配的影響范圍。
OK，到這里，Bundle Adjustment的求解方法，我們基本就介紹完了。BA真是個好東西，建議大家多參考一些相關的資料。
值得一提的是，盡管我們說Bundle Adjustment可以同時調整相機內參、地圖點和相機姿態。但是在ORBSLAM2中，我們會把地圖點都固定住，並固定第一幀的相機姿態，利用地圖點作為約束來優化其它相機姿態，所以這個問題其實更像是姿態圖優化——顧名思義，就是一個只優化相機姿態的問題。
另外，筆者發現了一個非常好的博客：https://www.cnblogs.com/xiaochen-qiu/p/9768949.html，這篇博客是泡泡機器人的邱博士寫的，供大家參考。
ORBSLAM2閉環方法提綱
按照慣例，我們還是同樣，再來談談ORBSLAM2中，他做閉環的思路是什么樣的，以供大家參考。
1. 首先需要關閉正在執行的全局BA；
2. 更新與當前幀的共視圖關鍵幀，共同觀測不小於15個點；
3. 為共視圖關鍵幀各保留兩個姿態，一個是原始姿態，一個是經回環校正以后的姿態；
4. 利用校正后的姿態將共視圖關鍵幀所有地圖點的位置全部更新，即利用舊姿態投影回像平面，再用新姿態投影到三維空間；
5. 更新當前幀共視圖中的每個關鍵幀各自的共視圖；
6. 融合地圖點，將當前幀的所有與回環幀相關聯的地圖點用回環幀的地圖點進行替換，若未匹配的，則直接構建新地圖點，用回環幀對應位置的地圖點初始化；
7. 利用更新的相機姿態將回環幀的共視圖關鍵幀構成的局部地圖點全部重投影到當前幀中，為當前幀查找最優匹配，實現更好的融合；
8. 更新共視圖關鍵幀與回環幀的連接，並剔除舊的連接關系；
9. 優化Essential Graph，優化具有較強連接關系的姿態，並通過增加不動姿態作為約束，來增強優化的效果；
10. 執行全局Bundle Adjustment，即把所有的地圖點和所有關鍵幀全部包括進來進行優化。值得注意的是，這里固定了第一個關鍵幀的姿態，固定了所有地圖點，只優化除第一幀外的所有關鍵幀的姿態，所以更像一個姿態圖優化。
於是，到這里，我們就把ORBSLAM2中的閉環模塊講完了。
總結
總結一下，本文主要介紹的是ORBSLAM2中的閉環模塊，內容涉及Bundle Adjustment。
由於在前面幾講中，或多或少介紹了Bundle Adjustment的思想，但是沒有涉及具體怎么利用稀疏性來求解問題，於是在這一講中，筆者便詳細地分析了這個過程。
此外，筆者羅列了閉環模塊的方法大綱，供大家在實際開發項目時參考。
OK，ORBSLAM2這個經典框架我們基本就分析完了。接下來，筆者可能會談談它的實際應用等問題，不過，那是后話了。
參考文獻：
[1] 視覺SLAM十四講
PS：
如果您覺得我的博客對您有所幫助，歡迎關注我的博客。此外，歡迎轉載我的文章，但請注明出處鏈接。
對本文有任何問題可以在留言區進行評論，也可以在泡泡機器人論壇：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技術交流模塊發帖提問。
我的github鏈接是：https://github.com/yepeichu123/orbslam2_learn。