入门理解关于SLAM中的优化

学习参考：SLAM14讲学习笔记（七）后端（BA与图优化，Pose Graph优化的理论与公式详解、因子图优化）_slam 因子图优化 卡尔曼 比较 区别-CSDN博客四.因子图优化学习---对因子图优化的粗浅理解_惯导因子图优化-CSDN博客

状态估计中，一般分为基于滤波的方法和基于优化的方法。

基于滤波分为：卡尔曼滤波和扩展卡尔曼滤波；

基于优化：BA优化，PoseGraph位姿图优化，因子图优化。

1.KF：从k-1时刻后验推k时刻先验，从k时刻先验推k时刻后验；

2.EKF：对卡尔曼滤波器进行修正，针对不是线性的情况，采用一阶泰勒展开近似线性。

3.BA优化：把一路上的所有坐标点与位姿整体放在一起作为自变量进行非线性优化

4.PoseGraph优化：先通过一路递推方式算出的各点位姿，通过数学方式计算得到一个位姿的变换A，再通过单独拿出两张图像来算出一个位姿变换B，争取让B=A

5.因子图优化：保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，需要重新计算的再去计算，从而避免冗余计算。

因子图优化：

贝叶斯图：

路标l和位姿x1，在状态方程h的作用下一起决定了一个观测数据z

位姿x1根据运动方程f和噪声u2一起构成了下一步的位姿x2，这个过程逐步递推，红框表示一次观测，篮框表示一次运动。

蓝色框可以写成：p(x3|x2,u3)

红色框可以写成：p(z1|x1,l1)

与KF内容串起来，可以将蓝框看成是先验，红框是似然，在似然和先验的共同作用下，才能得到后验，因此后端优化的目的就是最大化后验概率，最大化后验概率是通用最大化似然概率达到的：

通过进一步观察，发现最大后验概率由许多因子乘积而成，因此贝叶斯网络可以转化乘一个因子图（Factor Graph）。

上面的贝叶斯图变成因子图长这样

用圆圈表示变量，这里的变量是 SLAM中待优化的部分，即相机位姿 x 和路标 l，而没有观测 z 和输入 u——因为这几个量是给定的而不是待优化的。

所谓因子图优化，就是调整各变量的值，使因子乘积最大化。变量的概率取成高斯分布的形式。

因子图优化和图优化的区别在于：因子图是增量的处理后端优化。由于机器人是运动的，不同的边和节点会被不断加入图中。每加入一个点，普通图优化是对整个图进行优化，所以比较麻烦和耗时。因子图的话相当于是保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。以这种方式加速计算，避免冗余计算。

应用时：主要的难点是状态变量因子误差函数的构建和对应协方差矩阵的计算。这里使用了第三方库GTSAM，通过库中的API接口进行因子的添加、误差函数的构建、对应协方差矩阵的计算、因子图的优化求解等。GTSAM添加因子构建优化问题求解步骤如下：

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

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

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