二十三.激光和惯导LIO-SLAM框架学习之LIO-SAM项目工程代码介绍---基础知识

  专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架，环境搭建和跑通过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）_goldqiu的博客-CSDN博客

四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取_goldqiu的博客-CSDN博客

五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计_goldqiu的博客-CSDN博客

六.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---4.laserMapping.cpp--后端建图和帧位姿精估计（优化）_goldqiu的博客-CSDN博客

七.激光SLAM框架学习之A-LOAM框架---速腾Robosense-16线雷达室内建图_goldqiu的博客-CSDN博客

八.激光SLAM框架学习之LeGO-LOAM框架---框架介绍和运行演示_goldqiu的博客-CSDN博客

九.激光SLAM框架学习之LeGO-LOAM框架---速腾Robosense-16线雷达室外建图和其他框架对比、录包和保存数据_goldqiu的博客-CSDN博客

十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码_goldqiu的博客-CSDN博客

十一.激光惯导LIO-SLAM框架学习之LIO-SAM框架---框架介绍和运行演示

十二.激光SLAM框架学习之livox-loam框架安装和跑数据集_goldqiu的博客-CSDN博客_livox 数据集

十三.激光SLAM框架学习之livox-Mid-70雷达使用和实时室外跑框架_goldqiu的博客-CSDN博客

十四.激光和惯导LIO-SLAM框架学习之惯导内参标定_goldqiu的博客-CSDN博客

十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）_goldqiu的博客-CSDN博客

十六.激光和惯导LIO-SLAM框架学习之配置自用传感器实时室外跑LIO-SAM框架_goldqiu的博客-CSDN博客

十七.激光和惯导LIO-SLAM框架学习之IMU和IMU预积分_goldqiu的博客-CSDN博客

十八.多个SLAM框架（A-LOAM、Lego-loam、LIO-SAM、livox-loam）室外测试效果粗略对比分析_goldqiu的博客-CSDN博客

十九.激光和惯导LIO-SLAM框架学习之项目工程代码介绍---代码框架和一些文件解释_goldqiu的博客-CSDN博客

二十.激光、视觉和惯导LVIO-SLAM框架学习之相机内参标定_goldqiu的博客-CSDN博客

二十一.激光、视觉和惯导LVIO-SLAM框架学习之相机与雷达外参标定（1）_goldqiu的博客-CSDN博客

二十二.香港大学火星实验室R3LIVE框架跑官方数据集_goldqiu的博客-CSDN博客

基础知识：

因子图优化及GTSAM库介绍

因子图

在slam的后端优化问题中，通常会通过⼀些传感器的观测，比如视觉特征点，IMU预积分量，Lidar面点和边缘点（角点）的约束去构建一个优化问题，求解状态量（如位姿、速度等）。这个时候存在一个问题，当给这个系统新增⼀个约束时，就会重新建立所有的约束对状态量的优化问题进行求解；当优化模型增大时，显然进行一次优化的时间也会增加很多；一方面实时性遭遇了挑战，另一方面，很久之前的状态也没有继续更新的必要。为了解决这个问题，⼀种方式是使用滑动窗口来控制优化问题的规模，而滑动窗口需要处理好边缘化的问题；另一种方式，可以使用因子图的模型来解决这个问题。 Kaess等科研人员提出iSAM，即增量平滑和建图，使其可以自动增量处理大规模优化问题；具体来说，其内部使用一种基于概率的贝叶斯树，使得每次给因子图增加一个约束时，会根据贝叶斯树的连接关系，调整和当前结点“关系比较密切”的结点（概率比较大）；这样既保障了优化问题的求解精度，也使得耗时不会随着优化问题的增大而增大。关于因子图优化理论可以参考iSAM，iSAM2相关论文。

因子图中一些概念

变量结点：类似g2O中的顶点或者ceres中的参数块，代表需要被优化的变量。 因子结点：类似g2O中的边或者ceres中的cost function（代价函数），代表约束，如预积分约束、位姿先验约束、帧间位姿约束等。

GTSAM库的介绍

GTSAM全称是 Georgia Tech Smoothing and Mapping library，是佐治亚理工学院的科研人员基于因子图和贝叶斯网络推出的⼀个C++库文件，如果想在工程中使用因子图优化的相关算法，最常用的方式就是借助GTSAM这个库来实现，因为其内部已经封装了关于因子图优化以及iSAM相关的算法实现，只需要像调用其他第三方库的方式（如openCV，PCL等）调用GTSAM库即可。 关于GTSAM库的详细介绍可以参考其官方文档

https://gtsam.org/

GTSAM关于IMU预积分相关接口介绍

对两个雷达关键帧之间的若干帧IMU进行预积分，以形成预积分约束，对两帧之间的位置、速度、姿态以及零偏进行约束。GTSAM从4.0版本开始就在内部增加了IMU预积分相关的接口。

预积分相关参数：

gtsam::PreintegrationParams

对IMU数据进行预积分之前需要事先知道IMU的噪声，重力方向等参数。

跟预积分相关的计算在这个类中实现：

gtsam::PreintegratedImuMeasurements

这个类有⼀些重要的接口。

预积分量复位：

resetIntegrationAndSetBias

也就是清空之前的预积分量，重新开始一个新的预积分量。预积分的计算依赖一个初始的IMU零偏，因此在复位之后需要输入零偏值，这里的复位和重设零偏在一个接口里。

输入IMU的测量值，内部会自动实现预积分量的更新以及协方差矩阵的更新：

integrateMeasurement

预积分量的时间长度：

deltaTij

预积分量可以计算出两帧之间的相对位置、速度、姿态的变化量，根据预积分结果，结合上一帧的状态量就可以计算出下一关键帧的预测：

predict

预积分前端代码

变量结点

也是预积分模型中涉及到的三种状态变量

gtsam::Pose3 // 表⽰六⾃由度位姿
gtsam::Vector3 // 表⽰三⾃由度速度
gtsam::imuBias::ConstantBias // 表⽰IMU零偏

因子结点

先验因子：

gtsam::PriorFactor

表示对某个状态量T的一个先验估计，约束某个状态变量的值不会离该先验值过远。

imu因子：

gtsam::ImuFactor

通过IMU预积分量构造出的IMU因子，即IMU约束

状态量间的约束：

gtsam::BetweenFactor

约束相邻两状态量之间的差值不会距离该约束的值过远。

关于因子图的相关知识可以看因子图优化专栏：

因子图优化

关于IMU预积分的相关知识可以看这篇文章：

goldqiu：十七.激光和惯导LIO-SLAM框架学习之IMU和IMU预积分