因子图优化原理（iSAM、iSAM2）
- slam问题
- 通过贝叶斯网络对slam问题建模
- 从贝叶斯网络到因子图
- 非线性最小二乘问题求解
- isam1增量QR分解
- isam2
- 结语
slam问题
在介绍因子图之前，先从一个简单的slam问题入手，如下图所示：
在图中清晰的显示了各个节点和和连接结点边之间的定义，对于图结构不做过多说明，假定读者已经有一定的先验知识。在上图显示的slam问题中，实际上我们要解决的问题就是，从机器人的控制和观测中恢复机器人的路径以及环境地图。
通过贝叶斯网络对slam问题建模
在贝叶斯网络中，通过有向边连接变量，除了路标状态节点和机器人的状态节点外（两个状态节点为状态变量），还定义了路标观测节点（红色）和运动观测节点（绿色）（两个观测节点为观测变量）。这个贝叶斯实际上描述了一种状态和观测的联合概率模型。假设知道系统的状态变量X（机器人所在位置、路标点所在位置），可以推测出机器人得到的观测量Z（观测量是通过某些传感器得到的，而传感器有已知模型，可以通过数据手册得到），即已知机器人的状态量和传感器的模型，就可以推算出机器人的观测量。所以得到了如下图所示的一个生成模型：
由于观测量之间相互独立，可以得到下式所述的模型：
从贝叶斯网络到因子图
基于上文，我们知道贝叶斯网络是一个生成模型，解决的从状态变量得到观测观测变量，而我们的问题是从观测变量来推出状态变量，是一个状态估计问题，这就引出了因子图，因子图是一个inference model。
首先来对问题做一个推导：由于我们要从观测量来推出状态量，这是一个后验概率问题，结合贝叶斯丁玲，问题被描述为最大化一个后验概率问题（MAP），即给定系统系统观测量，求解系统状态量，使得下式的条件概率最大：
通过上面的定义将MAP问题转换成求解一个最优的状态，使得似然概率和先验概率的乘积最大化。
因子的定义推导：将先验和似然项因式分解作为因子：
将上述乘积形式的每一项表示成因子，将状态变量表示成节点，观测量表示成边，就转化成了下面的因子图。
对于上面的因子图，我们用下面的式子来描述：
其中红色的部分表示观测因子，绿色为里程计因子，紫色为先验因子。那我们要解决的问题是什么呢，就是寻找一个最优的X，使得这些因子图的乘积最大化。
因子图中每个因子的定义：在因子图中通常每个因子都是通过指数函数的形式来定义的，这是因为我们得到的观测量是有不确定性的，而且现实中这种不确定性通常服从高斯分布：
其中f(x)表示的是误差函数，当误差越小的时候，其指数函数越大，其定义如下图所示：
通过上面的说明，到这里我们终于得到了我们定义因子图之后要解决的问题了。我们先理一遍，首先我们介绍了贝叶斯网络，他是一个概率网络，通过贝叶斯网络可以解决已知状态变量来推出观测变量的问题，但是事实是我们通常可以得到观测变量，想要推出状态变量，所以我们推出了因子图，并且结合贝叶斯定理，将问题描述成一个最大后验概率问题。最终得到目标函数：
现在有了目标函数之后，剩下的最后一个问题就是，如何求解这个最优化问题。对于目标函数的右边取负对数，可将最大化因子的成绩问题转化成一个非线性最小二乘问题：
通过求解这个非线性最小二乘的问题就能得到问题的最优解。
非线性最小二乘问题求解
对于上式的最小二乘问题，可以通过高斯牛顿迭代法来求解，首先给定X一个初始值（通常在slam问题中这个初始值我们使用上一时刻得到的最有状态估计X），在给定这个变量的基础上再去求一个修改量，最后在用这个初始值加上这个修改量得到新的状态X，重复迭代这个过程，直到收敛。其流程如下图所示：
对于上图中的非线性最小二乘问题，对中间的式子做一阶泰勒展开，就可以将其转化为线性最小二乘问题：
现在的问题就转化成了如何求解这个线性最小二乘问题，对于一个线性最小二乘问题是由比较成熟的方法来得到线性最小而成问题的最优解，比较常见的方法为Normal equation：
对于Normal equation问题可以通过Cholesky分解来求解：
也可以通过QR分解来求解：
下面通过最开始的简单的slam问题来说明一下因子图的求解过程：
上图中的雅可比矩阵的每一行对应一个因子，右边的矩阵是一个信息矩阵，在现实中生成的因子图所获得的雅可比矩阵是稀疏矩阵，这是一个很好的性质，非常有利于线性系统的求解：
但是这个矩阵的稀疏性和变量的order有关，order不一样的话，矩阵的稀疏性体现也不一样，会出现下图所示的情况：
这种情况下不利于线性系统的求解，所以通常需要一些算法来对信息矩阵进行重排序来维持其好的稀疏性，目前较为成熟的算法为COLAMD。通过上面的描述我们知道了对于一个确定的因子图如何求解。但是常见的问题（比如SLAM问题）生成的因子图是增量的，并且在机器人运行的过程中，因子图只是又很小一部分的改变，如果我们从头对因子图进行分解来求解的话，不利于系统的实时性，那么如何解决这个问题呢？下面介绍论文isam中是怎么做的。.
isam1增量QR分解
对于给定的J进行QR分解如下图所示：
现在我想对J加入几行，这几行的意义就是，当因子图中加入了新的节点和因子时在信息矩阵中的表现就是多了几行，问题就是怎么不在重新分解的基础上能得到新的R。
论文中给出了求解方法，公式如下图所示：
对于在R下面加了一行（非零元素）变成如下图所示：
其求解办法是通过givens rotation可以将R变成一个新的上三角矩阵：
但是随着因子的增加，也会带来一个问题，就是如果我新加的因子只影响上一时刻的因子的时候，这个矩阵依然是有很好的稀疏性，但是如果新加入的因子是回环，那么givens rotation讲影响之前的所有节点，使R矩阵变得稠密，如下图所示
这时候就体现出了isam1的问题，在经过以一定的时间后就需要对元素进行重新排序，将稠密矩阵变得稀疏：
isam2
在这里简单说明isam2是将因子图转换成贝叶斯网络去求解，具体的文章参考paper吧，我自己还没有研究透彻。但是我建议明白了因子图的求解流程之后，就不要再去看那枯燥的公式了，脑瓜子疼。其实他就是求解问题的一种工具，所以明白了问题之后，调库不香嘛？
结语
写这篇文章是因为，自己早早的就接触到了因子图优化，针对salm问题的因子图优化问题，我就一直想不通，这个后验概率是在哪里体现的呢，现在终于明白了，其实就是上面指数形式的因子推导的过程，雅可比矩阵是什么玩意呢？原来是矩阵的一阶倒数呀。所以大家在学习论文的时候不要害怕公式，因为公式看不懂你是真不懂，哈哈。