三.因子图优化学习---董靖博士在泡泡实验室的公开课学习

专栏系列文章如下：

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

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

先放个视频链接：

【泡泡机器人公开课】第五十六课：gtsam_tutorial-董靖_哔哩哔哩_bilibili

理论部分

SLAM属于链状的贝叶斯网络，当前的状态只跟上一个状态有关，路标点的测量值只跟当前状态和所对应路标点位置有关。
先验分布或者先验值可以预设或者通过GPS等有绝对位置测量值的传感器来得到。
最大后验概率正比于所有因子图相乘的最大概率值。
线性化之后得到的J矩阵每一列对应了因子图中一个节点，是要估计的状态（包括机器人自身的状态和路标点的状态）。每一行对应的是每一个测量值（包括路标点测量值和系统运动测量值），也就是因子。转换为线性代数问题后，J矩阵是稀疏的，所以是比较容易求解的，收敛效率比较高，解算比较快。有两种办法求解这个线性代数问题：1. QR分解（慢，求解稳定性好，特征值近似0，矩阵接近奇异还能求解）2.Cholesky分解（稳定性差，矩阵接近奇异可能会发散，快）
ordering：选择合适的J矩阵的列排序。GTSAM调用了COLAMD算法能求出近似最优ordering。
isam2：通过贝叶斯树的结构，增加了因子后重新优化不需要从初始值开始做起，利用之前已有的解、之前已经线性化好的贝叶斯网络，重优化的时间比较短，有些优化工作不用重新做，特别是在graph变化不是特别大时（比如机器人建图往前走，没有闭环检测）
先验因子（prior factor）或初始分布的作用：固定地图的位置，不让地图在空间中进行平移和旋转，即求解唯一。

编程部分

C++：

1.Build factor graph

设置noiseModel：传感器假设是高斯分布，对应的协方差矩阵就是noiseModel

BetweenFactor：里程计或系统运动学模型。里程计测量值相当于系统控制输入。

2.Give initial values

设置初值很重要，关系到能否跳出局部最优值，达到全局最优值，一般设置为比较接近最后的解的初始值。

3.Optimize

4.(Optional) Post process, like calculate marginal distributions

自定义因子：

定义输出的误差向量e决定了factor输出的cost function
定义J矩阵：最困难的部分
如何求J矩阵：GTSAM自动微分，因为误差向量形式很复杂，有很多函数嵌套。在GTSAM只需要提供每一步的函数和每一步的J矩阵，用嵌套的形式去自动求解。（通过自定义模板类的方式）

Matlab：

GTSAM在matlab有个toolbox，可以进行c++代码生成matlab代码（通过生成自定义matlab toolbox）

安装：

下载gtsam：https://github.com/borglab/gtsam.git

找到GTSAM_INSTALL_MATLAB_TOOLBOX,把OFF改为ON，然后编译安装

ubuntu下安装matlab，安装好后根据GTSAM安装路径配置toolbox（使用matlab设置路径添加该文件夹）

测试test_gtsam出现以下则成功：

下载demo程序：https://github.com/dongjing3309/gtsam-examples

运行如下：

应用

IMU ：预积分转换成关键帧之间的IMU factor

Structure-less factor by Schur complement ：四维重建

Multi-Robot SLAM

补充：

SLAM后端优化发展阶段：

滤波阶段
图优化阶段（全局一致性优化）
增量平滑优化阶段（因子图优化）

因子图优化的作用：

在slam的后端优化问题中，通常会通过⼀些传感器的观测，比如视觉特征点，IMU预积分量，Lidar面点和边缘点（角点）的约束去构建一个优化问题，求解状态量（如位姿、速度等）。这个时候存在一个问题，当给这个系统新增⼀个约束时，就会重新建立所有的约束对状态量的优化问题进行求解；当优化模型增大时，显然进行一次优化的时间也会增加很多；一方面实时性遭遇了挑战，另一方面，很久之前的状态也没有继续更新的必要。为了解决这个问题，⼀种方式是使用滑动窗口来控制优化问题的规模，而滑动窗口需要处理好边缘化的问题；另一种方式，可以使用因子图的模型来解决这个问题。

下一章开始讲GTSAM的使用。