深入理解SLAM中的Marginalization

寫在前面

本文主要是對於Decoupled, Consistent Node Removal and Edge Sparsification for Graph-based SLAM前半部分的解讀，論文可謂是短小精悍，個人花了很多時間去學習和理解，文章中不乏會有一些理解不到位的情況，歡迎各位大神們拍磚，一起學習進步。

---

Abstract

基於圖優化的SLAM方法已經取得了成功，但是圖的大小會隨着時間的增長而變得很大，從而變得computational costs。爲了解決這個問題，論文使用分開的邊緣化和稀疏化解決這個問題。同時，論文還做了以下兩個工作：

1. 論文驗證了在邊緣化的時候，構建密集因子時使用的線性化點要使用Markov blanket的相對位姿，而不是絕對位姿；
2. 在稀疏化的時候，論文通過在線的方法來確定最相似的稀疏結構，而不是通過離線的結構選擇；

---

Introduction

首先在實踐中，有兩種方法——Generic LInear Contraints(GLC)和Nonlinear Factor Recovery(NFR)。但是這兩個方法都是將邊緣化和稀疏化是耦合的，但是這種方法並不能保證所有的信息都能保存在稠密因子中，主要原因是因爲邊緣化和稀疏化都在同一時刻的話，稀疏化的線性化點並不是邊緣化之後的線性化點，也就是說，稀疏化的時候並沒有最大程度的利用稠密因子的信息。

所以在這個論文中，作者將這兩個操作解耦了，在邊緣化之後進行稀疏化，保證可以利用邊緣化留下的信息。最後，在稀疏化的過程中主要構建了一個稀疏正則的凸優化來在線決定如何進行圖的稀疏。本文的工作主要有：

• 在局部圖上（Markov blanket）進行邊緣化和稀疏化。在邊緣化的時候，在局部地圖上找到一個參考點，並求解最大似然問題，相比於使用全局的位姿，這樣的方法才能保證一致性。邊緣化之後的分佈將以非線性測量的形式爲後續的優化提供先驗信息；
• 操作的解耦一方面是爲了更好的利用邊緣化信息，另一方面是爲了更好的分散計算量。在稀疏化的工程中，首先對稠密分佈的內部團（團概念參考圖模型的相關文章）因子進行稀疏化（就是與被邊緣化幀相關的幀），注意這些約束要在稀疏化之前被排除出來，這樣保證稀疏化之前能對這些測量進行再次的線性化（相當於也沒有丟掉，只是以另一種形式存在了）；然後構建一個L1正則凸優化來決定最終要優化的稀疏結構，這個結構旨在獲得一組最好的反應Markov blanket中全部信息的稀疏測量值，然後以此來代替稠密因子；

PS: Markov blanket

直譯爲馬爾科夫毯子，也有翻譯爲馬爾科夫覆蓋域，一個例子如下圖1。因爲這個概念是全文得以推導的基礎，因此這裏有必要着重說一下：

1. wiki百科對Markov blanket解釋爲：“the Markov blanket for a node in a graphical model contains all the variables that shield the node from the rest of the network. This means that the Markov blanket of a node is the only knowledge needed to predict the behavior of that node and its children”，我個人的理解是說Markov blanket要包含一些node，這些node是爲了做預測所需要的全部信息，而網絡中的其他信息都可以被屏蔽。
2. 在下圖中，可以看到如果我們以節點A來建立一個Markov blanket，那麼此時A的父節點、兄弟節點以及子節點都是影響A預測的因素，因此他們和A一起構成了一個Markov blanket。
3. 下面的例子其實是一個有向圖，SLAM中的節點屬於無向圖，因此構建Markov blanket的時候，不需要考慮兄弟節點。

---

Related Work

爲了解決基於圖的SLAM系統的計算量，人們嘗試了很多種方法，但是邊緣化是一種最穩妥的方法，因爲這樣可以很好的保留之前的信息不被丟失，但是邊緣化之後會導致圖變得稠密，但是可以通過近似的方法減少這部分填充。

GLC 方法在邊緣化節點的markov blanket上應用邊緣化，但使用了內部團的測量，從而產生n元線性約束來近似覆蓋域。 但是，這些線性因子是在全局狀態下被估計的，這可能導致測量的不一致性。

NFR方法不同，他的因子並不限制爲線性的。這些因子的信息矩陣是通過最小化近似邊緣分佈與原始邊緣分佈的KL散度來獲得的。除了使用全局的狀態來進行線性化，作者也嘗試了局部狀態的估計作爲線性化點，但是作者並沒有嚴格的論證。

上述兩個方法有一個共同的特點，就是他們的近似都是採用預先決定的稀疏結構對邊進行加權的方法，並不是在線的計算這個稀疏的結構。

文章爲了解決上述的問題，首先是把邊緣化和稀疏化進行了解耦，其次就是在線的計算稀疏結構，最後就是論證了使用局部的估計是最好的辦法。

---

Graph-based SLAM

這個章節就是簡單介紹了一下整個基於圖優化的SLAM的方法，主要是下面三個公式：

1. 觀測方程，觀測認爲是滿足一個高斯分佈
   $\mathbf{z}_{i j}=\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)+\mathbf{n}_{i j}$

2. 最大似然問題：
   $\hat{\mathbf{x}}=\arg \min _{\mathbf{x}} \sum_{(i, j) \in \operatorname{supp}(\mathbf{z})}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2} \tag{1}$

3. 最大似然轉換爲非線性最小二乘問題，使用高斯牛頓方法如下：
   $\delta \mathbf{x}^{(k)}=\arg \min _{\delta \mathbf{x}} \sum_{(i, j) \in \operatorname{supp}(\mathbf{z})}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\hat{\mathbf{x}}_{i}^{(k)}, \hat{\mathbf{x}}_{j}^{(k)}\right)-\mathbf{J}_{i j}^{(k)} \delta \mathbf{x}\right\|_{\Lambda_{i j}}^{2} \tag{2}$

4. 更新狀態變量：
   $\hat{\mathbf{x}}^{(k+1)}=\hat{\mathbf{x}}^{(k)}+\delta \mathbf{x}^{(k)}$

---

Marginalization

首先構建一個Markov blanket，其中以$X_1$爲中心節點，如下圖2：

其中：

• 紅色的頂點代表被邊緣化的幀，表示爲$X_m=\{X_1\}$；
• 綠色的頂點代表關聯幀，表示爲$X_b=\{X_0, X_2, X_3\}$；
• 藍色的頂點表示無關幀，表示爲$X_r=\{X_4\}$；
• 紅色的測量表示與邊緣化幀相關的測量，表示爲$Z_m=\{Z_{01}, Z{_{12}}\}$；
• 綠色的測量表示與關聯幀相關的測量，表示爲$Z_c=\{Z_0, Z_{03}, Z_{23}\}$；
• 藍色的測量表示與無關幀相關的測量，表示爲$Z_r=\{Z_{04}, Z_{34}\}$；

當Markov blanket建立完成之後，就把邊緣化的節點在局部圖上邊緣化掉，概率推導公式如下：
$p\left(\mathbf{x}_{b} | \mathbf{z}_{m}\right)=\int_{\mathbf{x}_{m}} p\left(\mathbf{x}_{b}, \mathbf{x}_{m} | \mathbf{z}_{m}\right) \mathrm{d} \mathbf{x}_{m}=: \mathcal{N}\left(\hat{\mathbf{x}}_{b}, \mathbf{\Lambda}_{t}^{-1}\right) \tag{3}$
實際中相當於僅僅用$X_b, X_m, Z_m$構建最大似然估計問題，然後把$X_m$邊緣化掉，下面就是邊緣化的理論推導了：

邊緣化求解$X_b$的先驗分佈

實際上，對於求解公式（3）的時候，我們需要使用貝葉斯公式求解一個最大後驗的問題，但是因爲並沒有$X_m，X_b$的先驗信息（我個人覺得不管是局部還是全局，都沒有這部分的先驗），所以最終求解的還是一個最大似然的問題（$p(X_m, X_b|Z_m) \propto p(Z_m|X_b, X_m)$）：
$\begin{array}{l} {\left\{\hat{\mathbf{x}}_{b}, \hat{\mathbf{x}}_{m}\right\}=\arg \max _{\mathbf{x}_{b}, \mathbf{x}_{m}} p\left(\mathbf{z}_{m} | \mathbf{x}_{b}, \mathbf{x}_{m}\right)=} \\ {\arg \min _{\mathbf{x}_{b}, \mathbf{x}_{m}} \sum_{(i, j) \in \operatorname{supp}\left(\mathbf{z}_{m}\right)}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2}} \end{array} \tag{4}$
其中信息矩陣爲：
$\boldsymbol{\Lambda}=\sum_{(i, j) \in \operatorname{supp}\left(\mathbf{z}_{m}\right)} \mathbf{J}_{i j}^{\top} \boldsymbol{\Lambda}_{i j} \mathbf{J}_{i j}=:\left[\begin{array}{cc} {\boldsymbol{\Lambda}_{m m}} & {\boldsymbol{\Lambda}_{m b}} \\ {\boldsymbol{\Lambda}_{b m}} & {\boldsymbol{\Lambda}_{b b}} \end{array}\right]$
其中$J_{ij}$使用局部估計的線性點，使用Schur補就可以得到邊緣化之後的$X_b$的信息矩陣，如下：
$\boldsymbol{\Lambda}_{t}=\boldsymbol{\Lambda}_{b b}-\boldsymbol{\Lambda}_{b m} \boldsymbol{\Lambda}_{m m}^{-1} \boldsymbol{\Lambda}_{b m}^{\top}$
得到先驗信息之後，整個沒有邊緣化的MLE（最大似然估計）問題（公式1）就可以寫作下面帶先驗的非線性最小二乘問題（這個是一個引理，下面證明）：
$\hat{\mathbf{x}}=\arg \min _{\mathbf{x}}\left\|\hat{\mathbf{x}}_{b}-\mathbf{x}_{b}\right\|_{\Lambda_{t}}^{2}+\sum_{(i, j) \in \operatorname{supp}\left(\mathbf{z}_{r}, \mathbf{z}_{c}\right)}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2} \tag{5}$

引理：
如果Jacobian和信息矩陣都使用公式（4）所示的局部MLE估計的線性化點，那麼邊緣化的非線性化最小二乘問題可以最好的近似未邊緣化的MLE問題。

證明：

• 原始局部MLE問題的誤差形式：

$c(\mathbf{x})=c_{m}\left(\mathbf{x}_{m}, \mathbf{x}_{b}\right)+c_{r}\left(\mathbf{x}_{b}, \mathbf{x}_{r}\right)$

​ 其中$c_m$表示邊緣化邊的誤差項，$c_r$表示非邊緣化邊的誤差項；

• 求解使得誤差最小時的狀態變量：

$\min _{\mathbf{x}} c(\mathbf{x})=\min _{\mathbf{x}_{b}, \mathbf{x}_{r}}\left(\min _{\mathbf{x}_{m}} c\left(\mathbf{x}_{m}, \mathbf{x}_{b}, \mathbf{x}_{n}\right)\right)=\min _{\mathbf{x}_{b}, \mathbf{x}_{r}}\left(c_{r}\left(\mathbf{x}_{b}, \mathbf{x}_{r}\right)+\min _{\mathbf{x}_{m}} c_{m}\left(\mathbf{x}_{m}, \mathbf{x}_{b}\right)\right)$

​ 可以看到又因爲要邊緣化掉$X_m$狀態量，因此式子最後只把最小化$X_m$項添加進去了；

• 對$c_{m}\left(\mathbf{x}_{m}, \mathbf{x}_{b}\right)$進行近似，有如下形式：

$c_{m} \simeq c_{m}\left(\hat{\mathbf{x}}_{m}, \hat{\mathbf{x}}_{b}\right)+\mathbf{g}^{T}\left[\begin{array}{c} {\mathbf{x}_{m}-\hat{\mathbf{x}}_{m}} \\ {\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}} \end{array}\right]+\frac{1}{2}\left[\begin{array}{c} {\mathbf{x}_{m}-\hat{\mathbf{x}}_{m}} \\ {\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}} \end{array}\right]^{T} \mathbf{\Lambda}\left[\begin{array}{c} {\mathbf{x}_{m}-\hat{\mathbf{x}}_{m}} \\ {\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}} \end{array}\right] \tag{6}$

​ 其中$g$表示公式(1)對於狀態的求導（其實相當於是增量方程中的$b$項），即：
$\frac{\partial{e^2}}{\partial{X}} = 2e\frac{\partial{e}}{\partial{X}}=2e \begin{bmatrix} \frac{\partial{e}}{\partial{X_m}} \\ \frac{\partial{e}}{\partial{X_b}} \end{bmatrix} = \begin{bmatrix} g_{mm} \\ g_{mb} \end{bmatrix} \text{ where } e=\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)$
​ 然後信息矩陣還是也一樣，是$e^2$對狀態的求導，不過推導出來和上面的信息矩陣一樣（跟增量方程中的$H$一 樣），即：
$\boldsymbol{\Lambda}=\left[\begin{array}{cc} {\boldsymbol{\Lambda}_{m m}} & {\boldsymbol{\Lambda}_{m b}} \\ {\boldsymbol{\Lambda}_{b m}} & {\boldsymbol{\Lambda}_{b b}} \end{array}\right]$

• 隨後就是求導等於0，然後運行schur補，可以得到最優的$X_m$值爲：

$\mathbf{x}_{m}=\hat{\mathbf{x}}_{m}-\mathbf{\Lambda}_{m m}^{-1}\left(\mathbf{g}_{m m}+\mathbf{\Lambda}_{m b}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)\right)$

• 把上式代入原式中可以得到誤差僅與$X_b$的關係，如下：

$\min _{\mathbf{x}_{m}} c_{m}\left(\mathbf{x}_{m}, \mathbf{x}_{b}\right) \simeq \zeta+\mathbf{g}_{t}^{T}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)+\frac{1}{2}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)^{T} \mathbf{\Lambda}_{t}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)$

​ 需要注意的是這裏的誤差和上面的$c_{m}\left(\hat{\mathbf{x}}_{m}, \hat{\mathbf{x}}_{b}\right)$不一樣的，如果需要解的話，需要從$g_t$和$\Lambda$聯合求解出來， 不過因爲這個值在邊緣化之後，就固定了，所以可以忽略這個值繼續求解增量等值；

• 所以最終誤差項變爲：

$\begin{aligned} c_{r}^{\prime}\left(\mathbf{x}_{b}, \mathbf{x}_{r}\right) &=\mathbf{g}_{t}^{T}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)+\frac{1}{2}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)^{T} \mathbf{\Lambda}_{t}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right) \\ &+\frac{1}{2} \sum_{(i, j) \in \operatorname{supp}\left(\mathbf{z}_{r}, \mathbf{z}_{c}\right)}\left\|\mathbf{z}_{i j}-\mathbf{h}_{i j}\left(\mathbf{x}_{i}, \mathbf{x}_{j}\right)\right\|_{\Lambda_{i j}}^{2} \end{aligned} \tag{7}$

• 進一步，如果我們的線性化點是整個局部圖的最優值，那麼公式（6）中的$g$就會爲0，進而公式（7）中的$g_t$也爲0，所以公式（7）也就寫作公式（5）。但是實際上，我們在使用的時候依舊選擇公式（7）的形式，因爲我們並不能保證當時的線性化點就是最優的，如果是，那麼$g_t$爲0，也不會影響結果。

下面深入一點，來看一下我們得到了先驗之後，怎麼用這個先驗：

當狀態$X_m$被邊緣化掉之後，之後的優化我們求解公式（7）所示的誤差函數就可以了，後一項很熟悉，就是BA的誤差，這裏不再多講；對於先驗項，在K時刻，我們需要利用這個信息構建增量方程，過程如下：

1. 首先把先驗的部分提取出來，如下：
   $c_{b}^{\prime}\left(\mathbf{x}_{b}\right) =\mathbf{g}_{t}^{T}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)+\frac{1}{2}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right)^{T} \mathbf{\Lambda}_{t}\left(\mathbf{x}_{b}-\hat{\mathbf{x}}_{b}\right) \tag{8}$

2. 當有增量$\delta{x_b}$時，取泰勒二階展開式爲：
   $c_{b}^{\prime}\left(\mathbf{x}_{b}+\delta{x_b}\right)=c_{b}^{\prime}(x_b)+\frac{\partial{c_b^{\prime}}}{\partial{x_b}}\delta{x_b}+\delta{x_b}^T \frac{\partial^2{c_b^{\prime}}}{\partial{x_b}^2}\delta{x_b} \tag{9}$
   其中，一階偏導（後面增量方程中的b）和二階偏導（後面增量方程中的H）的形式如下：
   $\begin{aligned} \frac{\partial{c_b^{\prime}}}{\partial{x_b}}&=g_t^T+\Lambda_t(x_b-\hat{x}_b) \\ \frac{\partial^2{c_b^{\prime}}}{\partial{x_b}^2}&=\Lambda_t \end{aligned}$
   可以看到的是，在使用邊緣化之後的先驗時，增量方程中的b是要更新的，但是H部分不再更新，保持邊緣化時刻的優化方向；

3. 最後一點，在LM過程中，當我們計算出了新的$x_b$時，我們需要把這個值帶入到公式（8）中指導算法接受更新還是拒絕更新；

局部MLE問題

這個部分就和Markov blanket息息相關了，最重要的原因就是上述的推導都是在一個局部且認爲不受其他變量影響的條件下進行的（就是公式（4）中只能引入$Z_m$），所以得到的先驗信息也都是給局部graph的因子的，這也是爲什麼作者說不能使用全局graph進行邊緣化（因爲全局邊緣化的時候，會有非$Z_m$的影響）。

回到優化問題上來，通常的SLAM優化不可能僅僅建立在Markov blanket那麼一點兒數據上，一定是$X_m, X_b, X_r$一起在做優化，那麼此時我們如何用這個先驗信息呢？總不能把原本的緊耦合變爲分開求解的鬆耦合吧？

作者給出的方法就是將要被邊緣化的因子構成的Markov blanket給以某一個節點shift起來，我個人覺得這個操作有如下的優點：

• 由於shift操作，Markov blanket就有一個很穩定的參考點，不管這個參考點怎麼變化，內部都是一致的；
• 同樣，因爲shift起來了，整個Markov blanket中就只包含$Z_m$而不含其他信息了；

針對上面圖2所示的Markov blanket，我們把整個子圖以$X_0$爲參考幀shift起來，所以在這個過程中，我們認爲$X_0$是不變的，仿照公式（3），我們在shift之後的Markov blanket可以建立下面的未邊緣化的最大似然問題，如下：
$\left\{^{0} \hat{\mathbf{x}}_{i}\right\}_{i=1}^{3}=\arg \max _{\left\{0 \mathbf{x}_{i}\right\}_{i=1}^{3}} p\left(\left.\mathbf{z}_{m}\right|^{0} \mathbf{x}_{1},^{0} \mathbf{x}_{2},^{0} \mathbf{x}_{3}\right) \tag{10}$
然後同樣經過上面的推導，我們同樣可以得到先驗的形式如下：
$prior=\left\|\left[\begin{array}{l} {^0\hat{\mathbf{x}}_{2}} \\ {^0\hat{\mathbf{x}}_{3}} \end{array}\right]-\left[\begin{array}{l} {\mathbf{x}_{2} \ominus \mathbf{x}_{0}} \\ {\mathbf{x}_{3} \ominus \mathbf{x}_{0}} \end{array}\right]\right\|_{^0\Lambda_{t}}^{2}$
可以看到，這個先驗中，我們的線性化點是邊緣化時刻，局部圖中的相對位姿。

作者在這個部分也說，很顯然，我們推斷的這個先驗信息對於局部的幀而言，是一個線性關係（$x_b-\hat{x}_b$），但是放在全局來看，先驗則變爲了非線性，因爲這個先驗中不包含非$Z_m$的信息。

個人感想

那麼我們可以看到，其實邊緣化這個操作需要注意的方面無非兩個點：

1. 在邊緣化的時候，要形成一個獨立的Markov blanket，不要引入非$Z_m$的約束影響；
2. 在後續優化的時候，一定要把局部的先驗考慮進來，我個人的感覺像是$X_b, X_r$正常構建BA優化，但是優化的過程中，$X_b$一定要轉回到局部座標系中看一下先驗（頗有種常回家看看的感覺）；

那麼這麼看來，個人看來港科開源的VINS-Mono似乎有問題：主要是在邊緣化時，VINS-Mono並非僅考慮了$Z_m $，也考慮了其他的約束；然後DSO在這方面做的就比較好了：在邊緣化的時候，DSO會僅僅使用那些跟要被邊緣化的幀有關的約束，其他的約束都不會參與到邊緣中過程中。

但是對於第二點，我個人並沒有很明顯的看到現有框架下的shift操作，難道都以座標原點進行shift了？