LIO-SLAM分析

参考文献：LIO-SAM源码解析：准备篇

一、代码框架

首先看看工程目录结构，只有四个cpp文件，从名字看也是简介明了，后面会分开对这四个文件进行源码的阅读。


对照LIO-SAM（）作者给出的下面这张系统架构图，我们首先获取一个整体上的印象。

对于一个SLAM系统后端优化是一个核心模块，有较早的卡尔曼滤波器、现在流行的图优化、因子图优化。LIO-SAM则采用因子图优化方法，包含四种因子。
LIO-SAM因子：IMU预积分因子，激光里程计因子，GPS因子，闭环因子。

上图时LIO-SAM的因子图结构，变量节点时关键帧，相邻的关键帧之间，通过IMU数据计算预积分，获取位姿变换，构建IMU预计分因子，也通过雷达子图的关键帧构建雷达里程计因子。每个关键帧再通过GPS数据参与修正，如果有闭环出现，闭环之间可以构建约束，关键帧之间有若干普通帧，这些帧不参与图优化，但是会执行scan-to-map的配准，优化每帧位姿。

整体流程：由以上两个图分析可知
1、激光运动畸变矫正，利用当前帧起止时刻之间的IMU数据、IMU里程计数据计算预积分，得到每一时刻的激光点位姿，从而变换到初始时刻激光点坐标系下，实现校正。
2、提取特征。对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征。
3、scan-to-map匹配。提取局部关键帧map的特征点，与当前帧特征点执行scan-to-map匹配，更新当前帧的位姿。
4、因子图优化添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿。
5、闭环检测:在历史关键帧中找候选闭环匹配帧，执行scan-to-map匹配，得到位姿变换，构建闭环因子，加入到因子图中一并优化。

下面在整体上给出这四个文件对应模块的功能，以及模块之间数据的发布-订阅关系。

(1) ImageProjection 激光运动畸变校正
功能简介

1、利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）；
（激光帧的起始数据，由IMU得到旋转增量，由IMU里程计得到平移增量，通过位姿增量，变换当前激光点到原始激光点，实现校正。）

2、同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。
订阅

订阅原始IMU数据；
订阅IMU里程计数据，来自ImuPreintegration，表示每一时刻对应的位姿；
订阅原始激光点云数据。
发布

发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；
发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtraction进行特征提取。

(2) FeatureExtraction 点云特征提取
功能简介

对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）。

订阅

订阅当前激光帧运动畸变校正后的点云信息，来自ImageProjection。

发布

发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变校正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等，发布给MapOptimization；
发布当前激光帧提取的角点点云，用于rviz展示；
发布当前激光帧提取的平面点点云，用于rviz展示。

(3) ImuPreintegration IMU预积分

TransformFusion类

功能简介

主要功能是订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）。

订阅

订阅激光里程计，来自MapOptimization；
订阅imu里程计，来自ImuPreintegration。

发布

发布IMU里程计，用于rviz展示；
发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。

2. ImuPreintegration类

功能简介

用激光里程计，两帧激光里程计之间的IMU预计分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）;
以优化后的状态为基础，施加IMU预计分量，得到每一时刻的IMU里程计。

订阅

订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预计分量，预测每一时刻（IMU频率）的IMU里程计；
订阅激光里程计（来自MapOptimization），用两帧之间的IMU预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。

发布

发布imu里程计；

(4) MapOptimization 因子图优化

功能简介

1、scan-to-map匹配：提取当前激光帧特征点（角点、平面点），局部关键帧map的特征点，执行scan-to-map迭代优化，更新当前帧位姿；
2、关键帧因子图优化：关键帧加入因子图，添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿；
3、闭环检测：在历史关键帧中找距离相近，时间相隔较远的帧设为匹配帧，匹配帧周围提取局部关键帧map，同样执行scan-to-map匹配，得到位姿变换，构建闭环因子数据，加入因子图优化。

订阅

1、订阅当前激光帧点云信息，来自FeatureExtraction；
2、订阅GPS里程计；
3、订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。
发布

1、发布历史关键帧里程计；
2、发布局部关键帧map的特征点云；
3、发布激光里程计，rviz中表现为坐标轴；
4、发布激光里程计；
5、发布激光里程计路径，rviz中表现为载体的运行轨迹；
6、发布地图保存服务；
7、发布闭环匹配局部关键帧map；
8、发布当前关键帧经过闭环优化后的位姿变换之后的特征点云；
9、发布闭环边，rviz中表现为闭环帧之间的连线；
10、发布局部map的降采样平面点集合；
11、发布历史帧（累加的）的角点、平面点降采样集合；
12、发布当前帧原始点云配准之后的点云；
论文分析参考：LIO-SAM探秘
：精细分析

2、论文分析：

摘要：

1、本文提出了一种紧耦合激光-惯性里程计方法，通过优化四个因子（LIDAR里程计因子，IMU预积分因子，GPS因子和回环因子）来得到机器人全局一致的位姿。
2、使用帧-局部地图匹配代替LOAM的帧-全局地图匹配。提高了帧图匹配的效率。

解决的问题

论文认为loam系列文章存在一些问题
1、将其数据保存在全局体素地图中
2、难以执行闭环检测
3、没有结合其他绝对测量（GPS）
4、当体素地图变得密集时，在线优化过程的效率变低。

解决的方法

使用因子图的思想优化激光SLAM，引入四种因子
1、IMU预积分因子
2、激光雷达里程计因子
3、GPS因子
4、闭环因子

A System Overview

我们首先定义在整篇论文中使用的坐标系和符号。 我们将世界坐标系表示为W，将机器人主体坐标系表示为B。为方便起见，我们还假定IMU坐标系与机器人主体坐标系重合。 机器人状态x可以写成：

B IMU Preintegration Factor

使用公式定义来自IMU的角速度和加速度的测量值。 2和3

现在，我们可以使用来自IMU的测量值来推断机器人的运动。 机器人在时间t +Δt处的速度，位置和旋转可以计算如下：

C. Lidar Odometry Factor

1.粗计算新帧与前关键帧的相对位姿变换，按阈值提取关键帧，五个关键帧合成一次区域的体素化点云地图。e,p代表edge和plane。M代表合成点云地图，F代表单关键帧点云，取∪集是合成。

2.edge和plane进行匹配。
3.优化也是常规优化。点对线和面对面来优化变换矩阵

D. GPS Factor

对于GPS因子。作者提出。

1.GPS测量值的时间戳根据里程计时间戳进行线性插值。
2.无需不断添加GPS因子。
3.当估计的位置协方差大于接收的GPS位置协方差时，添加GPS因子。
4.GPS在z方向可信度较低。

这里计算协方差是提取0,7,14号位置，根据ros手册和协方差知识，只是指xyz的测量方差。因为常见GPS为50HZ，所以短时间内能测多次，可以求方差。

E. Loop Closure Factor

1.使用的是一种简单但有效的基于欧氏距离的闭环检测方法。
2.闭环系数对于校正机器人高度的漂移特别有用，因为GPS的海拔高度测量非常不准确。