四.因子图优化学习---对因子图优化的粗浅理解

专栏系列文章如下：

一.因子图优化学习---董靖博士在深蓝学院的公开课学习（1）_goldqiu的博客-CSDN博客

二.因子图优化学习---董靖博士在深蓝学院的公开课学习（2）_goldqiu的博客-CSDN博客

三.因子图优化学习---董靖博士在泡泡实验室的公开课学习_goldqiu的博客-CSDN博客_因子图优化c++
 

对于因子图的isam1和isam2论文，核心就是增量QR分解和贝叶斯树。

增量QR分解

对于高斯牛顿法中J(x)矩阵构成的线性增量方程组的求解方式是矩阵分解。J(x)矩阵是由误差函数f(x)决定的，而在SLAM问题中J(x)矩阵是维数很高的矩阵。但由于J(x)矩阵的稀疏特性，利用QR分解求解J(x)矩阵构成的高维矩阵的效率就变得很高。

在机器人运行的过程中，因子图一般是逐渐成长和增大的。对于逐渐往外延伸的一个因子图，在机器人往前移动的过程中，增加了一组节点，随着观测的数据越来越多，增加了越来越多的因子进来。每一次求解因子图都是比上次求解的因子图大一些，而且大部分因子图与之前的因子图是基本一致的，这是增量推理问题。如果从零开始求解优化问题，所求矩阵会越来越大，求解速度会越来越慢，而且绝大多数都是重复性的工作。所以这里采用增量QR分解或贝叶斯树的方法来构建因子图，这分别对应了isam1和isam2。

isam1实际上做的就是增量QR分解。对于增量推理问题，有：

 ，对A矩阵做QR分解，即A = QR，有：

 

当增加一个新的观测量(因子)时：

进行QR矩阵更新：

因为系统增加了新的因子而导致R矩阵的下一列增加了非零元素，通过对R矩阵左乘Givens旋转矩阵，将增加的列上的非零元素置为零，假设旋转矩阵为Φ ： 

最后得到一个密集的三角阵，需要重新对状态变量进行排列，将矩阵重新转换为稀疏矩阵，以便后面优化的计算。

贝叶斯树

而isam2利用贝叶斯树进行线性因子图推理，将因子图转换成树状结构，如果是增加了少量因子，只需要增加少许树上的节点，不需要重新求解整个树，求解速度比较快。所有的数据都在树结构上，从树的根部开始，受影响了多大范围，就更新多大范围。但如果遇到很大的回环，因为大部分变量都有触及，所以贝叶斯树中的很多元素会被移动，有大量节点需要重新计算。这就是贝叶斯树构建的因子图优化方法。贝叶斯树是理论最优的方法，实现起来比较难；而增量QR分解是偏经验的方法，可以根据需求来选择采用isam1的增量QR分解还是isam2的贝叶斯树。但这两种方法在遇到大回环的情况下增加了大量因子，都需要重新进行求解。

应用：

根据原理和系统设计思路设计C++源程序，并配置在ROS-Catkin环境中，其中主要的难点是状态变量因子误差函数的构建和对应协方差矩阵的计算。这里使用了第三方库GTSAM，通过库中的API接口进行因子的添加、误差函数的构建、对应协方差矩阵的计算、因子图的优化求解等。GTSAM添加因子构建优化问题求解步骤如下：

1. 构建因子图：首先定义误差模型，然后加先验因子和其他变量因子，通过表示不同传感器的误差函数和协方差矩阵，构建不同类型传感器的因子，最后给因子设定初值。

2. 优化求解：选择优化器对因子图进行优化。

3. 对优化求解进行后处理工作。