参考文献：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的海拔高度测量非常不准确。

LIO-SLAM分析相关推荐

八种常用激光雷达和视觉SLAM算法的评估与比较
文章:Evaluation and comparison of eight popular Lidar and Visual SLAM algorithms 作者:Bharath Garigipati ...
∇SLAM：自动可微分SLAM
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 1 摘要将表示学习方法与同时定位和建图(SLAM)系统相结合是一 ...
FAST_LIO_SAM 融入后端优化的FASTLIO SLAM 系统前端：FAST_LIO2 后端：LIO_SAM
FAST_LIO_SAM Front_end : fastlio2 Back_end : lio_sam Videos : FAST-LIO-SAM Bilibili_link Source code ...
SLAM总结(二)-数学基础之求导和线性方程求解
SLAM总结(二)-数学基础之求导和线性方程求解 1.求导:高数中常见的是一个函数对一个自变量求导,属于标量对标量求导.在SLAM问题中,函数是目标函数(残差项,约束项),一般包含多个函数,用多维列向 ...
2018_Semantic SLAM Based on Object Detection and Improved Octomap_note
注释 (2022/4/15 上午9:14:24) "ABSTRACT" (Zhang 等., 2018, p. 1) (pdf) 提出了什么: "In this pape ...
slam过去与未来1_what_salm
SALM_通俗理解本文首先从通俗易懂(个人理解)的角度来解释什么是SALM(同时定位与建图).首先我们要知道SLAM用来干什么,最本质的应用就是赋予机器人灵魂(自主移动),要实现真正像人一样的智 ...
觉SLAM的主要功能模块分析
视觉SLAM的主要功能模块分析一．基本概念 SLAM (simultaneous localization and mapping),也称为CML (Concurrent Mapping and L ...
SLAM的开源以及在移动端AR的适用分析
当前的开源方案当下部分总结引用自blog:http://blog.csdn.net/OnafioO/article/details/73175835文章总结很好没本文关于其在移动端方面加以总结,希望 ...
十八.多个SLAM框架（A-LOAM、Lego-loam、LIO-SAM、livox-loam）室外测试效果粗略对比分析
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)
本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \co ...

LIO-SLAM分析

一、代码框架

2、论文分析：

摘要：

解决的问题

解决的方法

A System Overview

B IMU Preintegration Factor

C. Lidar Odometry Factor

D. GPS Factor

E. Loop Closure Factor

LIO-SLAM分析相关推荐

最新文章

热门文章