五. 激光雷达建图和定位方案-开源SLAM

前面内容：

一. 器件选型心得（系统设计）--1_goldqiu的博客-CSDN博客

一. 器件选型心得（系统设计）--2_goldqiu的博客-CSDN博客

二. 多传感器时间同步方案（时序闭环）--1

三. 多传感器标定方案（空间同步）--1_goldqiu的博客-CSDN博客

三. 多传感器标定方案（空间同步）--2

三. 多传感器标定方案（空间同步）--3

四. 点云着色（真彩点云）方案

五. 激光雷达建图和定位方案-引言-CSDN博客

五. 激光雷达建图和定位方案-基础概念-CSDN博客

五. 激光雷达建图和定位方案-算法工具-CSDN博客

五. 激光雷达建图和定位方案-开源SLAM

LIO-SAM

LIO-SAM的特点是紧耦合：就是将雷达观测、IMU信息拿到一起做状态估计，一起估计包括位姿6个状态变量、bias 6个状态变量、velocity 3个状态变量，一共15个状态变量

imuPreintegration节点做的事：

有两个实例化对象，生成了4个线程（4个回调函数）；

TransformFusion中：订阅通过imu估计的雷达里程计信息(后面都称为imu里程计信息)，订阅最终优化后的里程计数据（没有增量插值）

TransformFusion这个类产生的数据没有被其它节点使用，只是单纯的为了rviz显示用，所以这个类可以去掉，不影响最后的建图结果

IMUPreintegration中：订阅IMU数据和最终优化后的里程计增量数据，进行因子图优化，估计IMU的bias，输出imu估计的雷达里程计信息(后面都称为imu里程计信息)

这里进行滑窗边缘化的操作是设置一个key变量来记录优化的因子的个数，当超过一定数量后进行边缘化，重置优化器和因子图

imageProjection节点做的事：

只有一个实例化对象，生成了3个线程（3个回调函数）

imageProjection中：订阅雷达数据、IMU数据、IMU里程计数据；获得初始位姿，进行点云去畸变（利用IMU对点云进行去畸变）和投影点云，生成一个自定义消息类型的点云数据（有序点云，有垂直和水平线序）。

featureExtraction节点做的事：

只有一个实例化对象，生成了1个线程（1个回调函数）

FeatureExtraction中：订阅自定义消息类型的有序点云信息，进行了角点和面点的特征提取，发出自定义消息类型的有序特征点云信息。

mapOptmization节点做的事：

有1个实例化对象，生成了3个线程（3个回调函数）

另外有两个线程，一个用于回环检测、一个用于全局地图显示

mapOptmization中：订阅回环、GNSS、自定义点云特征信息；进行点云的配准，将雷达里程计、回环检测、GNSS因子加入因子图进行优化；发布优化后的雷达里程计，包括增量的；发布地图等相关信息。

如何进行回环检测的？

构建关键帧，将关键帧的位姿存储。以固定频率进行回环检测。每次处理最新的关键帧，通过kdtree寻找历史关键帧中距离和时间满足条件的一个关键帧。然后就认为形成了回环。

形成回环后，历史帧周围25帧，构建局部地图，与当前关键帧进行icp匹配求解位姿变换。

lio-sam 认为里程计累计漂移比较小，所以通过距离与时间这两个概念进行的关键帧的回环检测。

FAST-LIO2

Fast-Lio 采用迭代误差状态卡尔曼滤波（iESKF）来实现紧耦合的 LIO。在滤波器的设计上，Fast-Lio 和 LINS 大体相同，但在卡尔曼增益的计算部分有所区别。众所周知，计算卡尔曼增益时需要对观测对误差状态的雅可比矩阵与自身转置的乘积求逆，当观测的维度较大时求逆将变得耗时，比如有 1000 个点时就需要对 (1000×6)*(1000×6)大小的矩阵求逆。针对此问题，Fast-Lio 采用的新的卡尔曼增益计算方法，需要求逆的矩阵的规模仅与状态量的规模相关，而非观测的规模。

Fast-Lio 兼容旋转式机械激光雷达和 Livox 固态激光雷达，首先对点云做特征提取，并用 IMU递推对特征点做去畸变，然后将激光观测和 IMU预测放到 iESKF 中估计当前时刻的 Position、Velocity、Orientation、Bias 和 Gravity 18维度状态量，所有这些状态量都是位于世界坐标系下的。

Fast-Lio2 相比于 Fast-Lio 使用了增量式的 ikd-Tree 数据结构（五. 激光雷达建图和定位方案-算法工具-CSDN博客）维护 local map，有效降低了对激光点观测查询近邻的耗时，进一步提升了效率；此外，不再提取特征点，所有的点都根据点到面的方式寻找关联；而在其他算法部分，两者差别不大。