iSAM: Incremental Smoothing and Mapping

iSAM原理

论文：《iSAM: Incremental Smoothing and Mapping》
增量优化器GTSAM基于iSAM的方法，该方法能够实现增量式全轨迹SLAM优化。

ISAM是基于《Square Root SAM Simultaneous Localization and Mapping via Square Root Information Smoothing》的增量SAM(Incremental Smoothing and Mapping)的全轨迹SLAM方法。

主要思想

ISAM 主要思路是landmark-based SLAM，基于SAM的增量求解思路。将非线性问题线性化，得到Ax=b的形式，通过因式分解，将问题转换为Rx=d的问题，其中R为上三角矩阵，从而通过回代法求得x。
其他技术点：
使用块排序法(基于colamd的块元素排序，认为pose即point为一个整体块变量)来加速因式分解的过程。
对于新的变量观测，对R进行补充操作，使用Givens rotations重新将新的R阵化为上三角阵，从而继续用回代法进行求解。
周期变量重排序与增量SAM，保证随着时间推移，R阵的稀疏性，以及运算效率。

问题描述

假设一个SLAM系统，用the Bayesian belief net work表示如下图：

图中，SLAM问题的贝叶斯信念网络表示。$x_i$是机器人在时刻$i$的状态(位姿)，$l_j$指代地标$j$的坐标，$u_i$是时刻$i$系统的控制输入，$z_k$是第$k$个地标观测值。
$$\begin{gathered} X=x_i,i\in 0,\dots M; \\ L=l_j,j\in 1,\dots N; \\ Z=z_k,k\in 1,\dots K. \end{gathered}$$

所有变量和测量值的联合概率:
$$P(X,L,U,Z)\propto P({\mathbf{x}}_0)\prod _{i=1}^{M}P({\mathbf{x}}_i|{\mathbf{x}}_{i-1},{\mathbf{u}}_i)\prod _{k=1}^{K}P({\mathbf{z}}_k|{\mathbf{x}}_{i_k},{\mathbf{l}}_{j_k})$$
其中：
$P(x_0)$是初始状态的先验概率;
$P(x_i|x_{i-1}，u_i)$是由控制输入$u_i$参数化的运动模型;
$P(z_k|x_{i_k}，l_{j_k})$是地标测量模型。

状态转移方程及观测方程均符合高斯模型：
$$\begin{gathered} {\mathbf{x}}_i=f_i({\mathbf{x}}_{i-1},{\mathbf{u}}_i)+{\mathbf{w}}_i \\ {\mathbf{z}}_k=h_k({\mathbf{x}}_{i_k},{\mathbf{l}}_{j_k})+{\mathbf{v}}_k \end{gathered}$$

利用最大后验问题(MAP)求解变量(机器人位姿及路标点坐标)，MAP问题可以逐步转化为最小二乘问题：
$$\begin{array}{rl}& X^{\ast },L^{\ast }=\arg \underset{X,L}{\max }P(X,L,U,Z)\\ & =\arg \underset{X,L}{\min }-\log P(X,L,U,Z).\\ & =\arg \underset{X,L}{\min }\{\sum _{i=1}^M\Vert f_i({\mathbf{x}}_{i-1},{\mathbf{u}}_i)-{\mathbf{x}}_i{\Vert }_{{\Lambda }_i}^2\\ & +\sum _{k=1}^K{\Vert h_k\left({\mathbf{x}}_{i_k},{\mathbf{l}}_{j_k}\right)-{\mathbf{z}}_k\Vert }_{{\Gamma }_k}^2\end{array}$$

其中，最小二乘问题使用马氏距离构成：${\Vert \mathbf{e}\Vert }_{\Sigma }={\mathbf{e}}^T{\Sigma }^{-1}\mathbf{e}$

如果过程模型$f_i$和测量函数$h_k$是非线性的，并且没有一个好的线性化点，则使用非线性优化方法，如高斯-牛顿或Levenberg-Marquardt算法，该算法求解一系列线性近似值以接近最小值。
这类似于使用扩展卡尔曼滤波器(EKF)求解SLAM的方法，但允许多次迭代以收敛，从而避免了由错误的线性化点引起的问题。

当测量公式有好的线性化点(或者使用迭代法求变量更新量时)， 问题可以化成以下标准最小二乘问题：
$${\mathbf{\theta }}^{\ast }=\arg \underset{\mathbf{\theta }}{\min }{\Vert A\mathbf{\theta }-\mathbf{b}\Vert }^2$$

求解方法

ISAM将雅可比矩阵A进行QR分解来完成上述问题的求解，
为什么不用Cholesky分解：而Cholesky分解对信息矩阵$A^{T}A$进行分解，虽然$A^{T}A$维数较少，分解速度块，但其需要额外进行$A^{T}A$的计算。
而QR分解直接对A进行分解。
$$A=Q\left[\begin{array}{c}R\\ 0\end{array}\right]$$
$A\in {\mathbb{R}}^{m\times n}$，$A$为雅可比矩阵，行数与观测数保持一致，列数与待求解的变量数保持一致。
即$m$表示观测数，$n$表示待求解的变量数。

$R\in {\mathbb{R}}^{n\times n}$为上三角矩阵，$R^{T}R=A^{T}A$
$Q\in {\mathbb{R}}^{m\times m}$，为正交矩阵，从而问题可以转化为：
$\begin{array}{rl}{\Vert A\mathbf{\theta }-\mathbf{b}\Vert }^2& {\Rightarrow \Vert Q[\begin{array}{c}R\\ 0\end{array}]\mathbf{\theta }-\mathbf{b}\Vert }^2\\ & ={\Vert Q^{T}Q\left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }-Q^T\mathbf{b}\Vert }^2\\ & ={\Vert \left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }-\left[\begin{array}{c}\mathbf{d}\\ \mathbf{e}\end{array}\right]\Vert }^2\\ & ={\Vert R\mathbf{\theta }-\mathbf{d}\Vert }^2+{\Vert \mathbf{e}\Vert }^2\end{array}$

进一步简化得：
$$R{\theta }^{\ast }=d$$
而后可以根据回代法逐个对变量进行求解。

增量求解

SAM是一个全轨迹方法(即没有变量或观测会被marg)，随着时间的推移，A阵会变的越来越大，ISAM提出增量更新R阵的方法，当新的观测来到时，仅需要对R阵中相关变量行列进行增量补充，而后使用Givens Rotations 将新增的下三角非零元素转换为0，从而重新构成上三角矩阵进行求解。

增量更新

如下公式所示，可以直接将新的观测雅可比添加到R下，从而得到新的R‘阵，此时R’阵在新添加的行上出现下三角元素不为0的情况。
$$\left[\begin{array}{cc}{Q}^T& \\ & 1\end{array}\right]\left[\begin{array}{c}A\\ {\mathbf{w}}^T\end{array}\right]=\left[\begin{array}{c}R\\ {\mathbf{w}}^T\end{array}\right],\text{new RHS:}\left[\begin{array}{c}\mathbf{d}\\ \gamma \end{array}\right]$$
雅可比矩阵A的行对应观测，从而，当新的观测到来时，新的雅可比($w^T$)及残差$\gamma$，会分别被添加到A阵及向量b下，成为新的行。得到$A^{\prime }x=b^{\prime }$，即：
$$\left[\begin{array}{c}A\\ {\mathbf{w}}^T\end{array}\right]\mathbf{x}=\left[\begin{array}{c}b\\ \gamma \end{array}\right]$$
$$[\begin{array}{cc}Q& \\ & I\end{array}]\left[\begin{array}{c}A\\ {\mathbf{w}}^T\end{array}\right]\mathbf{x}=[\begin{array}{cc}Q& \\ & I\end{array}]\left[\begin{array}{c}b\\ \gamma \end{array}\right]$$
即
$$\left[\begin{array}{c}R\\ {\mathbf{w}}^T\end{array}\right]\mathbf{x}=\left[\begin{array}{c}d\\ \gamma \end{array}\right]$$

增量因式分解(基于Givens Rotations)

由于slam问题的稀疏性，R阵中新增的$w^T$相关行只与观测到的landmark以及当前时刻的pose有关系(非0)，此时仅需使用少的Givens Rotations 便可以将新的R阵的下三角非0元素变为0。
下图为新的R阵以及使用Givens Rotation进行增量因式分解的示意图。

我们的目的要将图中的x所在的块部分，及对角线上landmark相关的下三角部分进行消除。设$a_{ik}=x$，则我们选择如下的Givens Rotation:
$$\begin{array}{rl}& \Psi =(\cos \varphi ,\sin \varphi )\\ & =\{\begin{array}{ll}(1,0),& \text{if }\beta =0\\ \left(\frac{-\alpha }{\beta \sqrt{1+{\left(\alpha /\beta \right)}^2}},\frac{1}{\sqrt{1+{\left(\alpha /\beta \right)}^2}}\right),& \text{if }|\beta |>|\alpha |\\ \left(\frac{1}{\sqrt{1+{\left(\beta /\alpha \right)}^2}},\frac{-\beta }{\alpha \sqrt{1+{\left(\beta /\alpha \right)}^2}}\right),& \text{otherwise}\end{array}\end{array}$$

其中，$\alpha :=a_{kk},\beta :=a_{ik}$
经过一次Givens 旋转($\Psi R$)，上图中R阵红色相关行受到影响(左乘改变行)，我们再下一步将$a_{ik+1}$等元素进一步清0，从而便使用一定数量的Givens旋转将R阵重新上三角化。

假设，新的观测相关的变量在R阵中的存放顺序是先landmark，后pose。经过Givens重新三角化得到的R与原先的R相比，只有与当前观测相关的变量行受到影响，即特征点相关的对角矩阵块，以及最后(6)列对应的新的pose(6DOF)。如下图所示：

而后在使用回代法进行变量求解时，我们也只需求解与此次观测产生联系的变量进行求解，上图中经过重新三角化后最右侧的对应的红色变量。

注意，如果需要重新对变量进行排序时，先排序，后放pose；
此外，iSAM主要是基于landmark-based slam，因而其相关的示意图中，landmark多是已经出现过的，且使用的是分布问题来求解特征匹配关系。

$$[\begin{array}{cc}{Q}^T& \\ & 1\end{array}]\left[\begin{array}{c}A\\ {\mathbf{w}}^T\end{array}\right]=\left[\begin{array}{c}R\\ {\mathbf{w}}^T\end{array}\right]$$

由于slam问题的稀疏性，可以利用Givens rotation将下三角非零元素0化，从而得到新的上三角矩阵。

回环处理

iSAM通过变量重新排序(variable reordering)来避免大量的R的填充，使用启发式方法COLAMD有效地找到一个好的块排序。信息矩阵中列的顺序会影响变量的消除顺序，因此也会影响因子R中的条目数。
ISAM结合周期性变量重新排序以及增量快速更新保证R阵稀疏性，同时保证求解的效率。

上图表明了周期性排序与增量更新对R的稀疏性的影响以及求解耗时的影响，从结果可知，这两者的结合能够大幅度提高R阵的稀疏性，并缩短计算时间。

此外，将重线性化与周期变量重新排序相结合，以实现填充减少。换言之，仅在变量重新排序步骤中，我们还对测量函数进行了重新定义，因为无论如何都必须重构新的测量雅可比。

数据association

Data association主要内容包括：

1. JVC确定观测(新pose)与landmark之间的联系
2. pose 与 landmark之间的协方差确定及更新；用于构建马氏距离，形成观测(pose)与landmark之间的分配(association)问题。从而循环第一步求解该分配问题。
   即变量组合问题，并确定变量求解的置信度。

变量组合

该问题主要是解决观测与landmark的对齐问题，级确定当前观测到的是哪个landmark。ISAM使用的是maximum likelihood (ML)的分配问题。其构成最小二乘马氏association问题(分配问题的成本矩阵)如下：
$$\Xi =J\Sigma J^T+\Gamma$$
其中$\Gamma$表示测量噪声，$J$为观测方程的雅可比， $\Sigma$为变量(pose和landmark坐标)的协方差矩阵。
使用Jonker–Volgenant–Castanon (JVC)分配算法将每个观测分配给对应的landmark。

协方差

协方差矩阵表征了变量的不确定性以及各个变量之间的相关性：
对角线为各参数的不确定性(uncertainty)；
非对角元素表示各参数之间的相关性，即协方差。
$$\Sigma =\left[\begin{array}{cc}{\Sigma }_{jj}& {\Sigma }_{ij}^T\\ {\Sigma }_{ij}& {\Sigma }_{ii}\end{array}\right]$$

每当一次新的观测到来后，完整计算一次协方差成本是非常大的。新的观测往往只与少量的landmark以及当前的pose有关，即新的观测相关(interest的协方差的部分如下图所示：

当前关系仅与协方差矩阵中少数条目数据有关联。如在该示例中，与本次观测相关的协方差块包括：最新pose $x_2$与landmark$I_1$和$I_3$之间的边际协方差。通常需要计算的条目用灰色标记。

同样的假设：在最后添加最新的姿势，即首先添加新观察到的landmark，然后可选地执行变量重新排序，最后添加下一个pose。
由于对称性，计算协方差只需要对角线上的三角形块和最右侧的块列(6-DOF pose)。

即待求解的协方差分为两部分：

• 关于当前pose的最后一个列块，这里叫它Marginal Covariances；采用回代法得到。
• 以及landmark的不确定性(对角块阵)。由初始估计(Conservative Estimates)及每次更新得到。ISAM 仅利用初始估计计算landmark的不确定性。

1、 Marginal Covariances
我们知道信息矩阵的逆就是协方差，且根据因式分解，可以知道信息矩阵$A^{T}A=R^{T}R$, 我们提取$(R^{T}R{)}^{-1}$的最后一个列块(设维数为$d_x$)，便是协方差的最后一个列块，设其为$X$。
设：
$$B=\left[\begin{array}{c}{0}_{(n-d_{\mathbf{x}})\times d_{\mathbf{x}}}\\ I_{d_{\mathbf{x}}\times d_{\mathbf{x}}}\end{array}\right]$$
利用$(R^{T}R{)}^{-1}B$便可以提取到协方差的最后一个列块(右乘改变列),
即$$(R^{T}R{)}^{-1}B=X\Rightarrow R^{T}RX=B$$
继而问题可以变成：
$$R^{T}Y=B,RX=Y$$
继而可以通过反向迭代进行求解。

2、Conservative Estimates
结构不确定性的保守估计是从初始不确定性中获得的。由于当向系统中添加新的测量值时，不确定性永远不会增加，因此初始不确定性提供了保守的估计。
初始的保守估计设定如下：
$${\tilde{\Sigma }}_{jj}=\bar{J}\left[\begin{array}{cc}{\Sigma }_{ii}& \\ & \Gamma \end{array}\right]{\bar{J}}^T$$
其中，$\hat{J}$是线性化反投影函数的雅可比矩阵，即观测函数的逆函数(观测为输入变量，landmark为输出结果)。${\Sigma }_{ii}$为上一节求最后列块得到的pose的不确定性。$\Gamma$仍旧为噪声。

协方差提取
这一节是额外的，与上一节对比，通过从信息矩阵$(A^{T}A)$的逆阵中提取landmark的对角块来计算${\Gamma }_{jj}$。
$$\Sigma :=(A^{T}A{)}^{-1}=(R^{T}R{)}^{-1}\Rightarrow R^{T}R\Sigma =I$$
从而得到：
$$R^{T}Y=I,R\Sigma =Y.$$
继而可以通过一次前后以及一次反向的回代法，可以得到所需的协方差。
然而由于协方差矩阵是稠密的，因此逐个计算是$O(n^2)$。我们需要的仅是对应块的计算：通过以下方法可以精确恢复协方差矩阵中${\sigma }_{ij}$与因子矩阵R=(r_{ij})中的非零条目一致的所有条目。

$$\begin{array}{rl}& {\sigma }_{ll}=\frac{1}{r_{ll}}\left(\frac{1}{r_{ll}}-\sum _{j=l+1,r_{lj}\ne 0}^n{r}_{lj}{\sigma }_{jl}\right)\\ & {\sigma }_{il}=\frac{1}{r_{ii}}\left(-\sum _{j=i+1,r_{ij}\ne 0}^l{r}_{ij}{\sigma }_{jl}-\sum _{j=l+1,r_{ij}\ne 0}^n{r}_{ij}{\sigma }_{lj}\right)\end{array}$$

上图提供了保守协方差和精确协方差的比较。
能看出，保守估计(consercative)是最快的，其次是高效提取(即公式${\sigma }_{ij}$那个)，全提取是及其耗时的。

补充知识

COLAMD排序算法

COLAMD（Column approximate minimum degree）排序方法是一种用于稀疏矩阵因子分解和解析的优化排序算法。它主要用于提高稀疏矩阵分解（如LU分解）的计算效率，特别是在大型稀疏矩阵上的应用，例如线性方程组求解、最小二乘法等。

原理

COLAMD 算法的核心思想是通过重新排序矩阵的列和行，来尽量减小分解时因子的填充量。填充量指的是在LU分解中，新生成的非零元素的数量。COLAMD 试图最小化因子分解中的填充量，从而提高计算效率和内存利用率。

步骤

COLAMD 算法的具体步骤如下：

1. 列入度和行入度计算：

   • 列入度（Column Degree）：每一列的非零元素个数。
   • 行入度（Row Degree）：每一行的非零元素个数。

2. 初始化：

   • 对于给定的稀疏矩阵，首先计算每一列的列入度和每一行的行入度。

3. 主元列选择：

   • 选择列入度最小的列作为当前主元列，如果有多个列入度相同，则选择行入度最小的。

4. 消除操作：

   • 使用当前选定的主元列，对矩阵进行消除操作，将该列与其它列进行交换，以减小后续因子填充的可能性。

5. 更新列入度和行入度：

   • 消除操作之后，更新涉及到的列的列入度和涉及到的行的行入度。

6. 重复步骤3-5：

   • 重复选择主元列、进行消除操作和更新入度，直到所有的列都被处理完毕。

7. 最终排序：

   • 最终的排序结果就是通过 COLAMD 算法得到的列的排列顺序，这个顺序被用于稀疏矩阵的LU分解。

COLAMD 算法通过选择合适的列顺序来减小因子分解过程中的填充量，从而提高了LU分解的效率和稳定性。它在许多数学和工程应用中都有广泛的应用，特别是在处理大型稀疏矩阵时能够显著地提升计算性能。

JVC assignment

Jonker-Volgenant-Castanon 算法是一种高效的求解指派问题（assignment problem）的方法。指派问题在诸如任务分配、资源分配、和交通优化等领域有广泛应用。算法通过对任务和资源之间的成本矩阵进行优化，找到最低总成本的指派方案。

算法步骤如下：

1. 初始化：设定初始成本矩阵 ( C )。
2. 行缩减：对每一行进行减法操作，使每行的最小值变为0。
3. 列缩减：对每一列进行减法操作，使每列的最小值变为0。
4. 标记零元素：寻找0元素，用最少的水平和垂直线覆盖所有的0元素。
5. 调整矩阵：
   • 如果覆盖的线的数量等于行或列的数量，找到最优解。
   • 否则，调整矩阵使更多的0元素出现，然后返回步骤4。

以下是一个简单的示例和 Python 程序来实现 Jonker-Volgenant-Castanon 算法：

示例
假设有三个任务和三个工人，成本矩阵 ( C ) 如下：

	任务1	任务2	任务3
工人1	9	2	7
工人2	6	4	3
工人3	5	8	1

Python 实现

我们将使用 scipy.optimize.linear_sum_assignment 方法来求解：

import numpy as np
from scipy.optimize import linear_sum_assignment

# 成本矩阵
C = np.array([[9, 2, 7],
              [6, 4, 3],
              [5, 8, 1]])

# 使用Jonker-Volgenant算法求解
row_ind, col_ind = linear_sum_assignment(C)

# 输出结果
print("任务指派:")
for i, j in zip(row_ind, col_ind):
    print(f"工人 {i+1} -> 任务 {j+1}")
    
print("最小总成本:", C[row_ind, col_ind].sum())

运行结果

任务指派:
工人 1 -> 任务 2
工人 2 -> 任务 3
工人 3 -> 任务 1
最小总成本: 9

在这个示例中，工人1被指派到任务2，工人2被指派到任务3，工人3被指派到任务1，总成本是9。

这个方法可以用于更大的成本矩阵，同样能高效地找到最优指派方案。