Square Root SAM论文原理

论文《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\iff 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\iff 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 }^{\ast }\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{\partial f_i(x_{i-1},u_i)}{\partial 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}\triangleq \frac{\partial h_k(x_{i_k},l_{j_k})}{\partial x_{i_k}}{|}_{(x_{i_k}^0,l_{j_k}^0)}{J}_k^{j_k}\triangleq \frac{\partial h_k(x_{i_k},l_{j_k})}{\partial l_{j_k}}{|}_{(x_{i_k}^0,l_{j_k}^0)}\leftarrow \end{gathered}$$

得到线性化后的方程：

$${\delta }^{\ast }=\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 }^{\ast }=A^{T}b$$$$\mathcal{I}\triangleq A^{T}A=R^{T}R$$$$R^{T}y=A^{T}b$$$$R^{T}y=A^T，R{\delta }^{\ast }=y$$
从而得：
$$R{\delta }^{\ast }=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\ast Q=I_{n\times n}$) 和 一个上三角矩阵$R\in C_{n\times n}$, 使得: $A=QR$.

文中$Q\in C_{m\times m}，Q^T\ast 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\Rightarrow x_3=3$$

然后是第二个方程：
$$x_2+4x_3=7\Rightarrow x_2+4(3)=7\Rightarrow x_2=7-12\Rightarrow x_2=-5$$

最后是第一个方程：
$$2x_1+3x_2+x_3=8\Rightarrow 2x_1+3(-5)+3=8\Rightarrow 2x_1-15+3=8\Rightarrow 2x_1-12=8\Rightarrow 2x_1=20\Rightarrow 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^{\ast }=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方法难以解决的大型问题上也相当实用。当只有较少数量的变量受到线性化点变化的影响时，另一种方法是有利的。在这种情况下，可以应用降级和更新技术暂时从因子中删除这些变量，然后使用新的线性化点再次添加它们。