作者:AAAAaaaa叶子 | 来源:互联网 | 2023-08-28 08:11
专栏系列文章如下:
一.因子图优化学习---董靖博士在深蓝学院的公开课学习(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的使用。