gtsam 原理学习

gtsam一个高效的后端图优化库。

1. 基础知识

先要理解三个概念，因子图、贝叶斯网络、贝叶斯树。

1. 1因子图

具有多变量的全局函数因子分解，得到几个局部函数的乘积，以此为基础得到的一个双向图叫做因子图.

用图表示

其对应因子图为：
顶点:变量节点或函数节点，边线表示它们之间的函数关系。

因子图，无向图，与雅克比矩阵对应。

马尔科夫随机场，与信息矩阵对应。

1. 2贝叶斯网络

贝叶斯网络(Bayesian network)，又称信念网络(Belief Network)，或有向无环图模型(directed acyclic graphical model)，是一种概率图模型，于1985年由Judea Pearl首先提出。它是一种模拟人类推理过程中因果关系的不确定性处理模型，其网络拓朴结构是一个有向无环图(DAG)。

因子图通过消元算法，生成贝叶斯网络和平方根信息矩阵R。

灰色的为被消去的变量，红色为新产生的因子。 消元顺序为l1、l2、x1、x2、x3。

 这里用的方法为多波前乔里斯基分解，其实就是QR分解加舒尔补。

    消元顺序l1、l2、x1、x2、x3。

R矩阵可以增量分解，可以很容易进行边缘化。

固定滞后平滑器，只维护最后N个状态，其它的都边缘化掉。

1. 3 贝叶斯树

同时消元的过程也伴随着贝叶斯树的生成。贝叶斯树的生成原理，请查下如下博客https://blog.csdn.net/deepbodhi/article/details/119877754。

       非线性滤波与平滑：每添加一个因子，贝叶斯树只需要部分需要更新（增量更新）。当前端新加入一个观察或者关键帧时，产生一个新的因子f(xi, xj)，首先找到所有受影响的团，为起点是包含xi或 xj的团，终点是根团。含xi或 xj的团以下的支树，以及任何不包含含xi或 xj的支树不受影响。

    2 工作原理

        图优化，维护的是一个由节点和边组成的连接图。当前端新加入一个观察或者关键帧时，向图中添加一个边、或一个边和一个节点。此时图中需要判断是否产生了新的回环，如果产生，则构建最小二乘法，优化此回环。但此回环中的节点可能也处在别的环路中，因此需要将图中所有回环都进行一次优化。计算量可观。

        gtsam维护的一颗贝叶斯树。当前端新加入一个观察或者关键帧时，产生一个新的因子f(xi, xj)，首先找到所有受影响的团，为起点是包含xi或 xj的团，终点是根团。含xi或 xj的团以下的支树，以及任何不包含含xi或 xj的支树不受影响。

        然后更新贝叶斯树，将树中受影响的部分转换回因子图，将新的因子添加进去。再将这些临时因子图通过消元算法重新消元，生产成新的贝叶斯树，且之前没有受影响的支树也可以重新添加回去。

         里程计添加关键帧时，回环添加新的观测时，不区分，一样进行上面的处理找到贝叶斯树中需要更新的部分。然后重新因子化，添加新的因子，再消元。消元的过程产生平方根信息矩阵，求解更新量。但不是每添加一个因子都需要进行上述过程，在新加因子少时，可以用R矩阵的增量分解方法直接求解更新量。固定滞后平滑滤波，感觉对里程计添加顺序节点很有用。

        求解出的更新量，如要更新到每个节点的位姿吗？在每一个团上，我们都可以检测变量估计之间的差值，解的更新向下传播，直到差值小于一定的阈值，传播才停止。

     3. 部分细节

      3.1   消元顺序

近似最小度算法。度是指图中节点连接边的数量。最小度算法，指消元优先消去连接边最少的节点。这种方法，可以尽量减少边的填充（消元过程中相互独立的节点产生新的依赖）。

同时为了保证未来更新贝叶斯树时，受影响的部分尽量少，强制将最近访问的节点放到靠近尾部的地方即根团。具体原理请阅读论文《An_Approximate_Minimum_Degree_Ordering_Algorithm》。

     3.2   整体优化和增量优化

    整体优化和增量优化切换，固定N步，就整体优化一次。