gtsam 学习十（ISAM2 理论）

翻译自：iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree

摘要

提出了一种新型的贝斯树处理稀疏矩阵，在转化为因子图，可以更好的求解平方根信息和映射问题。
文中提出了三个概念

1. 使用概率密度项进行矩阵分解
2. 介绍如何将矩阵分解转换为贝叶斯数和条件概率密度
3. 基于贝叶斯树提供了一种新的系数非线性优化：ISAM2（进行重新排序和重新线性化，避免了批量更新）

问题描述

文章聚焦与如何用增量实时解决非线性问题，

1. 利用增量即新的测量值实时更新估计值？：比如机器人的导航和规划

2. 使用因子图求解一个状态估计文题
   （1）：定义一个因子图
   $F:因子图所有函数节点，f_i\in F$
   $\Theta ：因子图所有因子节点{\theta }_j\in \Theta$
   $E:边：两个因子图之间的关系$
   $G:一个因子图$
   $$G=(F,\Theta ,E)$$
   （2）：由因子图可以得到因式分解为：
   ${\Theta }_i:表示所有与函数f_i相邻因子节点{\theta }_j的集合$
   $e_{ij}:表示一个因子节点e_i与一个函数节点e_j的联系$
   $$f(\Theta )={\Pi }_i(f_i({\Theta }_i))$$
   （3）：得到目标函数为：
   $${\Theta }^{\ast }=argma{x}_{\Theta }f(\Theta )$$
   例如下图：
   $m:路标测量值$
   $c:回环$
   $u:里程计$
   

3. 假设测量模型为高斯模型
   $h_i({\Theta }_i):测量函数模型（理论值）$
   $z_i:测量值$
   $||e|{|}_{\sum }^2=e^T{\sum }^{-1}e$
   $$f_i({\Theta }_i)=e^{-\frac{1}{2}||h_i({\Theta }_i)-z_i|{|}_{{\sum }_i}^2}$$
   目标函数更新为：
   $$argmi{n}_{\Theta }(-logf(\Theta ))=argmi{n}_{\Theta }\frac{1}{2}||h_i({\Theta }_i)-z_i)|{|}_{{\sum }_i}^2$$
   定义协防差矩阵为（马氏距离）：
   $\Sigma :协防差矩阵$
   $$||e|{|}_{\Sigma }^2=e^T{\Sigma }^{-1}e$$

4. 使用高斯牛顿/列文伯格方法求解非线性方程:$h_i({\Theta }_i)$
   于是目标函数变为：
   $A:m\times n阶的雅克比矩阵，m：测量值的个数$
   $\Delta :n维向量$

   $$argmi{n}_{\Theta }(-logf(\Delta ))=argmi{n}_{\Theta }\frac{1}{2}||A\Delta -b)|{|}^2$$
   此时的协防差为:
   $$||\Delta |{|}_{\Sigma }^2={\Delta }^T{\Sigma }^{-1}\Delta ={\Delta }^T{\Sigma }^{-T/2}{\Sigma }^{-1/2}\Delta =||{\Sigma }^{-1/2}\Delta |{|}^2$$
   参数$\Delta$：
   迭代时，一旦找到当前的$\Delta$,使用$\Theta \oplus \Delta$作为下一次迭代值
   $\oplus :一般为加法，但是在用四元数做3D旋转和映射时使用相关李群代替$
   参数$A$：
   A为稀疏块矩阵，对应非线性高斯因子图在$\Theta$处的线性化，与原非线性图一样，均在$\Delta$处服从高斯分布，
   定义函数:$P(\Delta )$:
   定义函数:$P(\Delta )=e^{-logf(\Delta )}=e^{-\frac{1}{2}||A\Delta -b|{|}^2}$
   $A\Delta -b$:可以使用Cholesky或者QR分解：
   在$d\Delta ：\Delta 的微分为0时$:
   (1):Choleskyf分解得到$A^{T}A=R^{T}R$
   (2):$y=R\Delta$
   可以得到正规方程,先求解y，（或使用QR分解正规方程）就可以得到更新$\Delta$
   $$A^{T}A\Delta {=}^{(1)}{A}^{T}b⟺R^T{y}^{(1)}=A^b$$,

5. 最后得到GTSAM中的迭代求解方法(用直接方程求解器（Cholesky，QR）求解SLAM的平滑解的一般结构。)

(1):$\Theta$：
(2):${\Theta }^{\prime }=\Theta \oplus \Delta$:更新下一步
(3):step3-6步使用信赖区域法迭代求解

1. 添加测量值
2. 添加并初始化变量
3. 在(1)处线性化
4. 使用QR或者Cholesky分解
5. 求解Δ
6. 获得更新的估计值(2)

6.总结
上述迭代方法可以使用局部进行迭代，从而避免了批量处理，然而当加入了新的因子，变量顺序往往不是最优的，ISAM会进行周期性的批量分解，对变量重新排序并进行隐式分解，然而该方法只在批量处理中试探进行，不能得到线性最优解。

贝叶斯树

将状态估计文体直接转换为图模型解决，无需将因子图转换为稀疏矩阵，然后应用稀疏线性代数方法。

1. 将因子图转换为贝叶斯网络（变量消除）
   伪代码:变量消除法

(1):${\theta }_j$
(2):$f_{joint}({\theta }_j,S_j)={\Pi }_i{f}_i({\Theta }_I),({\Theta }_i:{\theta }_j相关函数节点)$
(3):$f_{joint}({\theta }_j,S_j)=P({\theta }_j|S_j)f_{new}(S_j)$

1.移除所有相邻与(1)相邻的函数节点，定义Sj，所有移除函数节点相关因子节点，除了(1)
2.得到联合概率密度矩阵(2)
3.使用链式规则，条件概率密度(3)，

消除顺序见下图
$原始函数节点:f(x_1),f(x_1,x_2),f(x_2,x^{}$