多传感器融合
目录
IMU 标定方法简介
因子图优化及 GTSAM 库介绍
GTSAM 库的介绍
GTSAM 关于 IMU 预积分相关接口介绍
预积分前端代码讲解
激光视觉融合方案 LVI-SAM 简介
视觉里程计 VS 激光里程计
LVI-SAM 激光视觉融合思路简介
多传感器融合工程实践经验与技巧
A-LOAM LeGO-LOAM LIO-SAM 不同算法的对比
算法中工程化技巧总结
多传感器融合算法改进落地建议
多传感器融合未来发展趋势
IMU 标定方法简介
无论是 VIO 还是 LIO，IMU 和其他传感器的标定结果往往对最终的里程记和建图性能
有着显著的影响，同样，在其他多传感器融合算法中，传感器之间的标定结果的精度
对多传感器融合的效果也是有着非常大的影响。
在 lidar-imu 融合的算法中，lidar 和 IMU 之间的外参标定也是非常重要，在一些数据集
上往往有着经过良好标定的结果，然而，绝大多数情况下，传感器之间的外参需要我
们自己去标定。
LIO-SAM 的工程中，作者推荐了一个 Lidar-IMU 标定的功能包
https://github.com/chennuo0125-HIT/lidar_imu_calib，来实现了一个简单的 lidar 和
IMU 之间的标定，通常来讲，lidar 和 IMU 之间的旋转外参想比于平移外参对里程记的
性能有着更显著的影响，因此，条件有限的情况下，可以只标定 lidar 和 IMU 之间的旋
转外参，平移外参则可以通过手工丈量等方式获取。
该旋转标定算法本质上是一个手眼标定算法，即使先计算出两帧 lidar 之间的旋转（通
过 ICP、NDT 等点云配准算法），然后计算出两帧 IMU 之间的旋转（IMU 陀螺仪积分），
然后通过手眼标定的方式求解出外参，其和 VINS-Mono 中相机 IMU 旋转外参初始化
的过程一致，具体方式为
因子图优化及 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 中跟 IMU 预积分相关的接口定义
gtsam::PreintegrationParams
预积分相关参数，我们对 IMU 数据进行预积分之前通常需要事先知道 IMU 的噪声，重
力方向等参数
gtsam::PreintegratedImuMeasurements
跟预积分相关的计算就在这个类中实现
这个类有一些重要的接口
(1) resetIntegrationAndSetBias
将预积分量复位，也就是说清空刚刚的预积分量，重新开始一个新的预积分量，因为
预积分的计算依赖一个初始的 IMU 零偏，因此，在复位之后需要输入零偏值，所以这
里复位和重设零偏在一个接口里。
(2) integrateMeasurement
输入 IMU 的测量值，其内部会自动实现预积分量的更新以及协方差矩阵的更新
(3) deltaTij
预积分量跨越的时间长度
(4) predict
预积分量可以计算出两帧之间的相对位置、速度、姿态的变化量，那结合上一帧的状
态量就可以计算出下一关键帧根据预积分结果的推算值预积分前端代码
该模块涉及到的变量结点
gtsam::Pose3 // 表示六自由度位姿
gtsam::Vector3 // 表示三自由度速度
gtsam::imuBias::ConstantBias // 表示 IMU 零偏
以上也是预积分模型中涉及到的三种状态变量涉及到的因子结点
gtsam::PriorFactor<T>
先验因子，表示对某个状态量 T 的一个先验估计，约束某个状态变量的状态不会离该先验值过远
gtsam::ImuFactor
imu 因子，通过 IMU 预积分量构造出 IMU 因子，即 IMU 约束
gtsam::BetweenFactor
状态量间的约束，约束相邻两状态量之间的差值不会距离该约束过远激光视觉融合方案 LVI-SAM 简介
LVI-SAM 是 Tixiao Shan 推出的一种视觉激光以及惯性融合的一个框架，他是由两个子模块
构成，激光惯 性融合框架 LIO-SAM，以及视觉惯性融合框架 VINS-Mono，这两个模块中
激光惯性模块是已经本次课程 重点讲解的内容，视觉惯性融合框架受制于时间原因，此章
节 仅 作 简 要 介 绍 ， 如 果 对 VINS-Mono 有 研 究 兴 趣 可 以 参 考
https://github.com/xieqi1/VINS-Mono-noted.git
视觉框架通常是在图像中提取特征点，以及通过光流或者描述子匹配的方式建立不同
帧特征点之间的关 联，通过三⾓化的方式恢复特征点的 3D 位置，随后在优化问题的
建模中，通常会把各个关键帧的位姿以 及特征点 3D 位置同时优化。 不同于基于激光
雷达的融合框架，视觉框架中一般无法直接获得尺度或特征点深度信息，因此，单目
视 觉 slam 问题中，尺度是模糊的。在 VIO 的框架中，我们可以通过 IMU 来提供尺度
上的观测和约束。 VINS-Mono 是由几个关键的模块组成
视觉前端
特征点的提取，同时通过光流追踪的方式建立两个连续视觉帧之间特征点之间的对应
关系
IMU 预积分
通过将两个连续视觉帧之间的 IMU 数据进行积分，形成两个视觉帧之间的预积分约束，
对两帧之间的位 姿，速度零偏等状态量形成约束
视觉惯性联合初始化
在整个系统初始化之前，视觉的尺度以及各个关键帧的状态是未知的，因此 VINS 使
用了一个松耦合求解 的方式，计算出初始的相机和 IMU 的外参，视觉尺度，各个关键
帧的状态以及重力方向等
基于滑动窗口的优化
系统初始化之后，系统就进入了滑窗优化求解，为了同时保证计算精度和计算效率，
VINS 采取了滑动窗 口的方式，及只优化最近的十个关键帧的状态，更早的关键帧将
被移出滑窗，同时以先验的方式继续影 响着滑窗内状态的优化，在整个优化建模中，
主要通过重投影约束和预积分约束两种视觉和惯性的约束 进行优化求解。
回环检测和位姿图优化
通过视觉词袋的方式进行回环帧检测，同时通过 PNP 的方式进行几何校验，一旦认
为是成功的回环之后 就会触发位姿优化，使得全局位姿一致性更佳
视觉里程计 VS 激光里程计
LIO-SAM 和 VINS-Mono 作为两款不同的 slam 系统，本质上都是实现位姿估计和地
图构建的目的，他俩相 比有一些共同和不同的地方
他们拥有着类似的框架，前端数据特征提取，后端位姿优化，同时加上各自的回环检
测模块和位姿图优 化模块以实现更高精度的建图和定位
然而，他们的主传感器不同，视觉 slam 更多是在图像上提取一些⾓点为基础，激光
slam 的特征则更多是 几何上的一些曲率独特的点，同时建立特征关联的方式也不同，
视觉更多通过描述子匹配或者光流追 踪，而激光更多是做的最近邻搜索来建立对应关
系。
同时由于视觉缺乏尺度，因此需要一个鲁棒的初始化过程来恢复尺度，同时由于尺度
的保持需要依赖加 速度计的观测，使用IMU 的方式耦合度更高，同时在匀速直线运动
情况下可能会尺度发散，而激光雷达 可以直接获取尺度信息，因此不需要过分复杂的
初始化以及运动场景的依赖。
视觉的场景识别能力更强，视觉的 DBOW 模型可以在没有位姿先验的情况下来搜索相
似度较高的图⽚作 为回环候选帧，而一般的激光雷达可能很难借助类似方式实现（当
然 scan context 或许也是一种方式）
LVI-SAM 激光视觉融合思路简介
我们知道视觉和激光 slam 各有其优缺点，如果他们能够取长补短，那岂不是更好？
LVI-SAM 就在这方面 进行了尝试，首先，由于单目VIO 通常需要一个比较复杂的初始
化过程，而激光 slam一般不需要特殊的 初始化（借助九轴 IMU 的情况下），因此，我
们可以使用激光 slam 前几帧的位姿输出来帮助视觉 slam 更 快更好的进行初始化。
视觉 slam 尺度问题不仅仅存在于初始化的过程中，还会存在其正常运行的过程中，
LVI-SAM 提出在提取 视觉特征点的过程中，通过相机和 Lidar 的外参，把 Lidar 点云
投影到相机平面中，帮助提取的特征点获 得真实尺度信息，从而避免退化等情况。
同时，由于 LIO-SAM 依赖一个 IMU 预积分节点来帮助 mapping 模块提供更好的先验
初值，不过这个节点 依赖后端 mapping 的定位结果，这个时候一旦 mapping 效果不
佳，则很有可能这种紧耦合方式会带来更 大的⿇烦，在 LVI-SAM 中，这个先验初值就
由 VINS-Mono 来提供，这样也就实现了建图模块和里程计模 块的解耦。
最后我们之前提到视觉的回环检测方式对初值没有要求，因此可能用来给 LIO-SAM 提
供回环检测，而避 免使用基于里程计的回环方案，在大场景下的建图需求中更为实用
和有效。
多传感器融合工程实践经验与技巧
A-LOAM LeGO-LOAM LIO-SAM 不同算法的对比
A-LOAM(LOAM)也即原始 LOAM，后续无论是 LeGO-LOAM 还是 LIO-SAM 都是基于
LOAM 做的改进版本，在 LOAM 中，作者根据多线激光雷达的性质，提出了激光雷达
的线特征和面特征的提取，并以此为基础实现了一个高频率低精度的前端以及低频率
高精度的后端，这一点和 PTAM 以来的视觉 slam 将 tracking 和 mapping 分为两个线
程有异曲同工之妙，借此完成了一个高精度的里程记和建图功能。
LeGO-LOAM 在 LOAM 的基础上进行了一些改进，如
1、利用车载激光大多水平安装的特征，提取出地面点
2、使用聚类算法，使得前端特征更为干净
3、前端使用两步优化方法，减少运算负载，使其在嵌入式平台上也能运行
4、后端引入关键帧概念，同时加入了回环检测
LIO-SAM 在上述基础上的改进
1、由于其支持手持设备，因此没有对地面点进行特殊处理
2、紧耦合的 lidar+IMU 融合模块，使得其充分利用 IMU 的数据，对快速旋转等场景有
着更好的鲁棒性
3、融合 GPS，使得全局地图可以在没有回环的情况下有着更好的全局一致性
4、易于扩展的框架，方便我们将其他传感器融合进来
总体来说，这三种框架随着时间顺序都是在前有基础进行的改造，因此，都是在吸收
现在框架基础上进行的改进
算法中工程化技巧总结
在上述一些框架中，我们可以看到一些工程话的技巧，比如
1、LeGO-LOAM 前端，对地面点的提取，利用地面点的一些性质对 roll，pitch 以及 z
进行一些约束和优化
2、通过 BFS 算法对前端特征进行过滤，使得更干净的特征留存了下来
3、后端滑动窗口的引入，使得构建局部地图更加方便，也更加方便实现纯里程记功能
4、对 GPS 的融合的谨慎的使用方式，使得既引入了全局观测，又不会对当前里程记
的平滑性产生负面影响
多传感器融合算法改进落地建议
多传感器融合的目的是取长补短，比如在以激光雷达为主的融合方案中，我们需要明
确激光雷达有什么缺陷，以此确定使用什么传感器进行融合
1、激光雷达需要运动补偿，我们需要短期内可靠的运动观测源，IMU 以及轮速就可以
被充分利用
2、激光雷达匹配本质上是一个优化问题，需要提供一个很好的初始值，和 1 一样，也
是需要可靠的短期运动观测源，紧耦合的 IMU 融合或者轮速也是非常好的处理方式
3、激光雷达频率不高，为了提高频率，我们需要高频的其他传感器以获得高频输出，
此时 IMU 和轮速又可以成为备选
4、里程记会有累计漂移的问题，全局观测是解决里程记该问题的非常好的方式，GPS
作为常见的全局定位传感器，提供了修正累计漂移的功能
多传感器融合未来发展趋势
人类需要多个传感器（眼睛鼻子耳朵等）来处理信息，SLAM 也是一样，对传感器的
正确使用会使得整个系统的鲁棒性越强，因此，在未来一段时间内，我们会尝试将更
多新的传感器应用到 SLAM 中来，比如最近几年推出的 LIVOX 激光雷达等，和现有传
感器一起取长补短，来提升整体性能的鲁棒性。
同时基于已有的传感器组合，我们也会探索更多紧耦合的融合方式，使得传感器之间
的融合更有效率，做出精度更高的 SLAM 系统