論文《Square Root SAM Simultaneous Localization and Mapping via Square Root Information Smoothing》

Square Root SAM論文原理

核心原理

關于SLAM問題的求解：
1、將非線性SLAM問題通過一階泰勒展開化成線性問題Ax=b，再通過對A或A^TA進行因式分解（factoration基于Variable Elimination），得到上三角矩陣R，構建新問題Rx=d。
通過回代（backward substitution）的方法進行求解。

2、本文說明，變量在雅可比矩陣的排列順序能夠影響因式分解的稀疏性，本文使用colamd的排序方法應用于SLAM塊結構（將位姿或特征點坐標均看成一個變量），增加因式分解及回代求解效率。

3、提出的smoothing approach的框架（利用a good order & factoration），即高效優化截止到當前幀的整個機器人的位姿及特征點。

SLAM問題的3種表示

貝葉斯網絡

在貝葉斯概率網絡表示中，機器人的狀態 $x$ 受頂部的馬爾可夫鏈限制，而機器人的環境在底部由一組地標$l$ 表示。中間層的測量值 $z$ 受機器人的狀態和測量的地標參數限制。
$$P(X,L,Z)=P\left(x_0\right)\prod _{i=1}^{M}P\left(x_i\mid x_{i?1},u_i\right)\prod _{k=1}^{K}P\left(z_k\mid x_{i_k},l_{j_k}\right)$$
其中：
$P(x_0)$表示先驗
$P(x_i|x_{i?1},u_i)$表示狀態傳遞概率
$P(z_k|x_{i_k},l_{j_k})$表示觀測概率

因子圖（Factor graph）

上圖中，位姿和地標分別對應于圓形節點，每個測量值對應于因子節點（黑色實心圓圈）
狀態傳遞方程與后驗概率方程：
$$x_i=f_i(x_{i?1},u_i)+w_i?P(x_i|x_{i?1},u_i)\propto \exp ?\frac{1}{2}\Vert f_i(x_{i?1},u_i)?x_i{\Vert }_{{\Lambda }_i}^2)$$

觀測方程與后驗概率方程：
$$z_k=h_k(x_{ik},l_{jk})+v_k?P(z_k|x_{ik},l_{jk})\propto \exp ?\frac{1}{2}|h_k(x_{ik},l_{jk})?z_k|{|}_{{\Sigma }_k}^2$$

馬爾科夫隨機場(Markov Random Field, MRF)

MRF 的圖形是無向的，沒有因子節點：其鄰接結構指示哪些變量由公共因子（度量或約束）鏈接。
P ( Θ ) ∝ ∏ i ? i ( θ i ) ∏ { i , j } , i < j ψ i j ( θ i , θ j ) ? 0 ( x 0 ) ∝ P ( x 0 ) ψ ( i ? 1 ) i ( x i ? 1 , x i ) ∝ P ( x i ∣ x i ? 1 , u i ) ψ i k j k ( x i k , l j k ) ∝ P ( z k ∣ x i k , l j k ) \begin{gathered} P(\Theta)\propto\prod_{i}\phi_{i}(\theta_{i})\prod_{\{i,j\},i<j}\psi_{ij}(\theta_{i},\theta_{j}) \\ \phi_0(x_0)\propto P(x_0) \\ \psi_{(i-1)i}(x_{i-1},x_i)\propto P(x_i|x_{i-1},u_i) \\ \psi_{i_kj_k}(x_{i_k},l_{j_k})\propto P(z_k|x_{i_k},l_{j_k}) \end{gathered} P(Θ)∝i∏??i?(θi?){i,j},i<j∏?ψij?(θi?,θj?)?0?(x0?)∝P(x0?)ψ(i?1)i?(xi?1?,xi?)∝P(xi?∣xi?1?,ui?)ψik?jk??(xik??,ljk??)∝P(zk?∣xik??,ljk??)?

SLAM最小二乘問題&線性化

結合狀態傳遞方程與觀測方程：
$${\Theta }^{?}\stackrel{\Delta }{=}\underset{\Theta }{\mathrm{argmin}}\left\{\sum _{i=1}^M\Vert f_i(x_{i?1},u_i)?x_i{\Vert }_{{\Lambda }_i}^2+\sum _{k=1}^K\Vert h_k(x_{i_k},l_{j_k})?z_k{\Vert }_{{\Sigma }_k}^2\right\}$$

待解未知量：各個時刻的位姿（截至當前時刻）：$x_i$， 觀測路標點的位置$l_j$
將狀態傳遞方程與觀測方程對變量進行線性化:
$$\begin{gathered} f_i(x_{i?1},u_i)?x_i\approx \left\{f_i(x_{i?1}^0,u_i)+F_i^{i?1}\delta x_{i?1}\right\}?\left\{x_i^0+\delta x_i\right\}=\left\{F_i^{i?1}\delta x_{i?1}?\delta x_i\right\}?a_i \\ {F}_i^{i?1}\stackrel{\Delta }{=}\frac{?f_i(x_{i?1},u_i)}{?x_{i?1}}{|}_{x_{i?1}^0} \end{gathered}$$

關于x_i的雅可比矩陣為單位陣。

$$\begin{gathered} h_k(x_{i_k},l_{j_k})?z_k\approx \left\{h_k(x_{i_k}^0,l_{j_k}^0)+H_k^{i_k}\delta x_{i_k}+J_k^{j_k}\delta l_{j_k}\right\}?z_k=\left\{H_k^{i_k}\delta x_{i_k}+J_k^{j_k}\delta l_{j_k}\right\}?c_k \\ {H}_k^{i_k}?\frac{?h_k(x_{i_k},l_{j_k})}{?x_{i_k}}{|}_{(x_{i_k}^0,l_{j_k}^0)}{J}_k^{j_k}?\frac{?h_k(x_{i_k},l_{j_k})}{?l_{j_k}}{|}_{(x_{i_k}^0,l_{j_k}^0)}\leftarrow \end{gathered}$$

得到線性化后的方程：

$${\delta }^{?}=\underset{\delta }{\mathrm{argmin}}\left\{\sum _{i=1}^M\Vert F_i^{i?1}\delta x_{i?1}+G_i^i\delta x_i?a_i{\Vert }_{{\Lambda }_i}^2+\sum _{k=1}^K\Vert H_k^{i_k}\delta x_{i_k}+J_k^{j_k}\delta l_{j_k}?c_k{\Vert }_{{\Sigma }_k}^2\right\}$$

該方程使用馬氏距離來定義：$$\Vert e{\Vert }_{\Sigma }^2\stackrel{\Delta }{=}{e}^T{\Sigma }^{?1}e=({\Sigma }^{?T/2}e{)}^T({\Sigma }^{?T/2}e)={\Vert {\Sigma }^{?T/2}e\Vert }_2^2$$

線性化方程的意義：

1. 好的線性化點（該點與最優解與殘差的變化成線性關系）直接將非線性問題轉為線性問題。
2. 使用迭代法完成求解時，使用該線性化方程求變量更新量，相當于一次牛頓法求\delta x

將整個整理成大矩陣：


A的維度為$(N{d}_x+K{d}_z)\times (N{d}_x+M{d}_l)$。

因式分解 factorization

求解上述線性方程，可以使用因式分解，構成Rx=d，其中R為上三角矩陣的形式，而后使用反向消元方法來完成求解。
1、 對$A^{T}A$進行Cholesky分解：
Cholesky 分解是把一個對稱正定的矩陣表示成一個下三角矩陣L和其轉置的乘積的分解。它要求矩陣的所有特征值必須大于零，故分解的下三角的對角元也是大于零的。Cholesky分解法又稱平方根法，是當A為實對稱正定矩陣時，LU三角分解法的變形。$A=L{L}^T$
原問題可化為：
$$A^{T}A{\delta }^{?}=A^{T}b$$$$\mathcal{I}?A^{T}A=R^{T}R$$$$R^{T}y=A^{T}b$$$$R^{T}y=A^T，R{\delta }^{?}=y$$
從而得：
$$R{\delta }^{?}=y$$

2、三角化LDL 分解
將信息矩陣三角化:
$$\mathcal{I}=R^{T}R=LD{L}^T$$

3、 QR分解
QR 分解將一個矩陣分解一個正交矩陣 (酉矩陣) 和一個三角矩陣的乘積. QR 分解被廣泛應用于線性最小二乘問題的求解和矩陣特征值的計算. A=QR。

$A\in C_{m\times n}$(m ≥ n，約束方程遠大于待求解變量個數). 則存在一個單位列正交矩陣 $Q\in C_{m\times n}$，(即 $Q?Q=I_{n\times n}$) 和 一個上三角矩陣$R\in C_{n\times n}$, 使得: $A=QR$.

文中$Q\in C_{m\times m}，Q^T?A=R$的補充。

$$Q^{T}A=\left[\begin{array}{c}R\\ 0\end{array}\right]Q^{T}b=\left[\begin{array}{c}d\\ e\end{array}\right]$$
對于密集矩陣A的分解，首選方法是從左到右逐列計算R。對于每一列j，通過將左邊的A與Householder reflection矩陣Hj相乘，將對角線下方的所有非零元素歸零。在n次迭代之后，A被完全因子化：
$$H_n..H_2{H}_{1}A=Q^{T}A=[\begin{array}{c}R\\ 0\end{array}]$$

得到分解矩陣（上三角矩陣R）后，可以將原先的最小二乘問題（LS problem）轉化為如下方程：
$$\Vert A\delta ?b{\Vert }_2^2={\Vert Q^{T}A\delta ?Q^{T}b\Vert }_2^2=\Vert R\delta ?d{\Vert }_2^2+\Vert e{\Vert }_2^2$$
即：
$$R\delta =d$$

以上問題可以通過反向消元（back-substitution）的方法進行求解

因此QR分解的計算復雜度主要由Householder reflections 矩陣的計算決定：即2（m-n/3）n^2。
將 QR 與 Cholesky 因式分解進行比較，我們發現兩種算法在 m>> n 時都需要 O（mn^2 ） 操作，但 QR 因式分解慢了 2 倍。雖然這些數字僅對密集矩陣有效，但我們已經看到，在實踐中，LDL 和 Cholesky 因式分解在稀疏問題上也遠遠優于 QR 因式分解，而不僅僅是常數因子。

矩陣與圖(Matrices ? Graphs)

雅可比矩陣A對應SLAM因子圖，表征觀測與變量之間的關系；
信息矩陣ATA對應markov random field因子圖，表征變量與變量之間的關系。
需要強調的是：SLAM問題的變量都是以參數塊作為一個整體的，如特征點的3維位置作為一個參數塊，6-DOF位姿作為一個參數塊。矩陣與圖的對應是塊參數的對應關系。

從而：
A 的塊結構與與 SAM 關聯的因子圖的鄰接矩陣完全對應。
信息矩陣 I = ATA 是與 SLAM 問題相關的馬爾可夫隨機場矩陣。同樣，在塊級別，ATA 的稀疏性模式恰好是關聯 MRF 的鄰接矩陣。MRF 中的節點對應于機器人狀態和地標。鏈接表示里程計或里程碑測量值。

在一些中，采用MRF圖視圖來揭示SLAM的濾波版本中固有的相關性結構。結果表明，當邊緣化過去的軌跡$x_{1:M?1}$ 時，信息矩陣不可避免地變得完全密集。因此，這些方法的重點是選擇性地刪除鏈接以降低濾波器的計算成本，并取得了巨大的成功。相比之下，這篇文章考慮了與平滑信息矩陣 I 相關的 MRF，該矩陣不會變得密集，因為過去的狀態永遠不會被邊緣化，SAM信息矩陣存放的是是完整的變量關系（從初始到當前）。

因式分解&變量消元(Factorization ? Variable Elimination )

QR分解和Cholesky（或LDL）分解都是基于變量消除算法的矩陣分解方法。在求解線性方程組、最小二乘問題等數值計算中，它們都涉及通過逐步消除變量來簡化問題。讓我們深入理解它們如何基于變量消除。

Cholesky分解（或LDL分解）

Cholesky分解將一個對稱正定矩陣 (A) 分解為一個下三角矩陣 (L) 及其轉置 (L^T) 的乘積，即 ($A=L{L}^T$)。LDL分解是Cholesky分解的一種變體，將矩陣分解為一個下三角矩陣 (L)，一個對角矩陣 (D)，和 ($L^T$) 的乘積，即 ($A=LD{L}^T$)。

變量消除在Cholesky分解中的作用

1. 逐步消除變量：通過逐步消去矩陣中的非對角線元素，將矩陣轉換為一個易于處理的形式。
2. 保持稀疏性：在處理稀疏矩陣時，通過選擇合適的消除順序，可以最大限度地保持矩陣的稀疏性，提高計算效率。

步驟

1. 從矩陣 (A) 的第一行和第一列開始，計算第一個對角元素和下三角矩陣 (L) 的第一列。
2. 對剩余的子矩陣進行類似的操作，消去非對角線元素，逐步構建下三角矩陣 (L) 和對角矩陣 (D)。
3. 重復這一過程，直到處理完所有元素。

QR分解

QR分解將一個矩陣 (A) 分解為一個正交矩陣 (Q) 和一個上三角矩陣 (R) 的乘積，即 (A = QR)。QR分解可以通過一系列的Givens旋轉或Householder反射來實現，這兩者本質上都是基于變量消除的過程。

變量消除在QR分解中的作用

1. Householder反射：通過引入Householder矩陣，將一個向量反射到某個子空間，使得消去矩陣中的某些元素。這個過程等價于逐步消除變量，簡化矩陣結構。
2. Givens旋轉：通過Givens旋轉，可以將矩陣的某些元素變為零，從而逐步將矩陣轉換為上三角形。這也是一種變量消除的形式。

步驟

1. 從矩陣 (A) 的第一列開始，通過Householder反射或Givens旋轉將下面的元素變為零。
2. 處理下一列，通過類似的操作消除非對角線元素。
3. 繼續這一過程，直到得到上三角矩陣 (R)。

QR分解和Cholesky分解（或LDL分解）都基于變量消除的思想，通過逐步消去矩陣中的非對角線元素，將問題簡化為易于處理的上三角或下三角形式。這些分解方法不僅用于求解線性方程組和最小二乘問題，還廣泛應用于各種數值計算和優化問題中。

選擇合適的消除順序對于保持矩陣稀疏性和提高計算效率至關重要。通過合理的變量消除，可以有效地簡化復雜問題，降低計算復雜度。

變量在信息矩陣及A陣中的排列序列即決定了變量消元（elimination）的順序（列驅動：從左到右）。
變量的排列序列影響因式分解的結果，影響R陣的稀疏程度。
使用反向消元求解的方法依賴R陣的稀疏性，當分解的結果R陣比較稠密時，此時使得因式分解的計算量變大，或者大于直接的A陣求逆。

變量計算（回代Back-substitution）

在得到R\delta=d的關系后，該等式示意圖如下：

回代（Back-substitution）是一種用于求解上三角矩陣線性方程組的算法。它通常用于解決通過Gaussian消去法或QR分解等方法得到的方程組。

基本概念

假設我們有一個上三角矩陣 (R) 和一個向量 (b)，并希望求解線性方程組 (Rx = b)。上三角矩陣的特點是它的下三角部分全是零，即所有的非零元素都位于對角線及其上方。

回代算法

回代的基本思路是從最后一個方程開始，逐步向上求解每個變量。具體步驟如下：

1. 初始化：從最后一個方程（倒數第一行）開始求解。
2. 逐步求解：根據上一個變量的值，向上求解前一個變量，直到所有變量都求出。

具體步驟

假設我們有一個 ($n\times n$) 的上三角矩陣 ($R$)，和一個 ($n$) 維向量 ($b$)，我們的目標是求解 ($Rx=b$) 中的 ($x$)。

1. 從第 ($n$) 個方程開始：
   $$R_{nn}{x}_n=b_n$$
   求解：
   $$x_n=\frac{b_n}{R_{nn}}$$

2. 對于第 ($k$) 個方程（從 ($n?1$) 到 1）：
   [ R_{kk} x_k + R_{k, k+1} x_{k+1} + \cdots + R_{kn} x_n = b_k ]
   已知 (x_{k+1}, \ldots, x_n) 的值，求解：
   [ x_k = \frac{b_k - \sum_{j=k+1}^{n} R_{kj} x_j}{R_{kk}} ]

例子

假設我們有以下上三角矩陣 (R) 和向量 (b)：

$$R=\left(\begin{array}{ccc}2& 3& 1\\ 0& 1& 4\\ 0& 0& 3\end{array}\right),b=\left(\begin{array}{c}8\\ 7\\ 9\end{array}\right)$$

我們從最后一個方程開始：
$$3x_3=9?x_3=3$$

然后是第二個方程：
$$x_2+4x_3=7?x_2+4(3)=7?x_2=7?12?x_2=?5$$

最后是第一個方程：
$$2x_1+3x_2+x_3=8?2x_1+3(?5)+3=8?2x_1?15+3=8?2x_1?12=8?2x_1=20?x_1=10$$

所以，解 (x) 是：
$$x=\left(\begin{array}{c}10\\ ?5\\ 3\end{array}\right)$$

回代是一種簡單而高效的方法，用于求解上三角矩陣的線性方程組。通過從最后一個變量開始逐步向上求解，可以快速找到解。這個過程在許多數值算法中，如Gaussian消去法和QR分解后，是不可或缺的步驟。

因式分解與變量順序

1、A good ordering
變量的排列順序影響因式分解的結果，下圖維用于良好排序的三角化（對信息矩陣進行分解得到的上三角R陣）圖（colamd）。這是一個有向圖，其中每條邊對應于 Cholesky 三角形 R 中的非零。

The triangulated graph for a good ordering (colamd):

Triangulated 用于說明經過因式分解三角化后的R對應的圖。

QR和Cholesky（或LDL）兩種分解方法都基于變量消元算法。這些方法的區別在于，QR 從因子圖中消除變量節點并獲得 $A=QR$，而 Cholesky 或 LDL 從 MRF 開始，因此獲得 $A^{T}A=R^{T}R$。兩種方法一次消除一個變量，從 $\delta 1$ 開始，對應于 $A$ 或 $I$ 的最左側列。消元的結果是，${\delta }_1$ 現在表示為所有其他未知數 ${\delta }_j>1$ 的線性組合，系數位于 $R$ 的相應行 $R_1$ 中。然而，在此過程中，連接到 ${\delta }_1$ 的所有變量之間會引入新的依賴關系，這會導致邊被添加到圖形中。然后以類似的方式處理下一個變量，直到所有變量都被消除。消除所有變量的結果是一個有向的三角化（弦chordal）圖。

2、LX ordering

一旦獲得了關于$R$的弦圖，就可以得到$R$的消元樹，它被定義為消元后的弦圖的深度優先生成樹，它有助于說明回代階段的計算流程。為了說明這一點，下圖顯示了通過使用眾所周知的啟發式方法獲得的和弦圖，該啟發式方法首先消除地標，然后處理姿勢（我們稱之為 LX ordering）。

LX ordering triangulated graph

相應的消除樹如下圖所示。樹的根對應于最后一個要消除的變量 ${\delta }_n$，這是在反向替換中要計算的第一個變量。然后，計算沿著樹向下進行，雖然這通常是以相反的列順序完成的，但不相交的子樹中的變量可以按任何順序計算，有關更詳細的討論。事實上，如果一個人只對某些變量感興趣，就沒有必要計算任何不包含它們的子樹。

LX-ordering 排序的消元樹，顯示了如何通過反向替換來計算狀態和地標估計值：首先計算根 - 這里是左側的第一個姿勢 - 然后沿著馬爾可夫鏈向右繼續。一旦計算出地標所連接的姿態，就可以計算出它們。

不同的參數排列順序會影響因式分解后的結果，R的稀疏性。參數的排列會影響因式分解的效率（求R的復雜度）以及變量回代的效率。對于中等大小的問題，一種流行的方法是 colamd。

左側是相關的雅可比 $A$ 測量值，其中有 $3\times 95+2\times 24=333$ 個未知數。行數 1126 等于（標量）測量值的數量。右邊：（頂部）信息矩陣 $\mathcal{I}=A^{T}A$;（中）其上三角形Cholesky三角形$R$;（下）通過更好的變量排序(使用colamd)獲得的替代因子 amdR。能夠看出使用colamd方法進行變量排序得到的R陣更為稀疏。

這篇文章將標量變量的集合（3維特征點位置以及6-DOF位姿）視為單個變量，并創建一個較小的圖形，該圖形封裝了這些塊之間的約束，而不是單個變量。在這個較小的圖形上調用 colamd 更高效。是關于SLAM的變量問題基于塊結構，colamd或任何其他近似排序算法都無法對塊矩陣進行處理。雖然對 colamd 性能的影響可以忽略不計，但我們發現，讓它直接在 SLAM MRF （塊結構上，位置和位姿視為單變量）上工作而不是在稀疏矩陣 $\mathcal{I}$ 上工作可以產生 2 倍到有時 100 倍的改進。

在某些情況下，任何排序都會導致相同的大填充。最壞的情況是完全連接的 MRF：從每個位置都可以看到每個地標。在這種情況下，消除任何變量將完全連接另一側的所有變量，然后完全知道集團樹的結構：如果先選擇位姿pose，則根將是整個地圖，一旦知道地圖，所有姿勢都將被計算出來。反之亦然，如果選擇了一個地標，則軌跡將是集團樹根集團，計算將通過（昂貴的）軌跡優化進行，然后（非常便宜的）計算地標。這兩種情況的解決方法依賴于塊標準分割逆或“舒爾補碼”。

SAM流程

Batch √ SAM

1. 構建雅可比 $A$ 和 RHS $b$ 測量。
2. 找到一個好的列排序 $p$，并重新排序 $A_p\leftarrow A$.
3. 使用 Cholesky 或 QR 分解方法求解 ${\delta }_p^{?}=argmi{n}_{\delta }||A_p{\delta }_p–b|{|}_2^2$。
4. 通過 $\delta \leftarrow \delta p$，最優解，并恢復原始排序：$r=p^{?1}$
   

Linear Incremental √ SAM

當有新的觀測到來時，信息矩陣會得到新的填充或更新：
$${\mathcal{I}}^{\prime }={\mathcal{I}}^{\prime }+a{a}^T$$

其中 T 是測量值雅可比 A 的新行，對應于在任何給定時間步長傳入的新測量值。但是，這些算法通常僅針對密集矩陣實現，因此我們必須使用稀疏存儲方案以獲得最佳性能。雖然存在稀疏的Cholesky更新，但它們的實現相對復雜。第二種可能性，易于實現并適用于稀疏矩陣，是使用一系列給定旋轉逐個消除新測量行中的非零。第三種可能性，我們在下面的模擬中采用，是更新矩陣 $\mathcal{I}$，并簡單地使用完整的 Cholesky（或 LDL）因式分解。雖然這似乎很昂貴，但我們發現，有了良好的排序，主要成本不再是因式分解，而是更新信息矩陣I。

例如，實驗結果表明，在運行結束時，通過稀疏乘法獲得 Hessian 的成本大約是因式分解的 5 倍。重要的是，信息矩陣包含了從開始到當前時刻所有的測量信息，因此無需在每個時間步長進行因式分解。原則上，我們可以等到最后，然后計算整個軌跡和地圖。然而，在實驗過程中的任何時候，地圖和/或軌跡都可以通過簡單的因式分解和反向替換來計算，例如，用于可視化和/或路徑規劃目的。

Non-linear Incremental √ SAM

如果在增量設置中，SLAM問題是非線性的，并且線性化點可用，則上述線性增量解決方案適用。是否是這種情況很大程度上取決于傳感器和機器人可用的先驗知識。例如，在室內移動機器人中，通常沒有良好的絕對方向傳感器，這意味著我們必須圍繞當前猜測的方向進行線性化。這正是EKF在更大規模環境中出現問題的原因，因為當過去的機器人姿勢從MRF中消除時，這些可能錯誤的線性化點被“烘焙到”信息矩陣中。平滑方法的一個顯著優點是，我們從不致力于給定的線性化，因為不會從圖形中消除任何變量。有兩種不同的方法可以重新線性化：在某些情況下，例如沿完整軌跡閉合一個大循環，重新線性化所有變量的成本更低。從本質上講，這意味著我們每次都必須調用上面的批處理版本。從好的方面來說，我們的實驗結果將表明，即使是這種看似昂貴的算法在EKF方法難以解決的大型問題上也相當實用。當只有較少數量的變量受到線性化點變化的影響時，另一種方法是有利的。在這種情況下，可以應用降級和更新技術暫時從因子中刪除這些變量，然后使用新的線性化點再次添加它們。