Simple-LIO-SAM: A C++ repository from zeal-up

文章解读

💟 Blog
🔯 知乎

本项目目的及特点介绍

LIOSAM源代码虽然不能说庞大，甚至可以说简单，因为整个代码库主要只要5个文件。但是里面每个模块都通过ROS的topic与其他模块有紧密的联系。整个的流程对新手十分不友好，下图是LIOSAM原本运行时的rqt_graph

第二个复杂之处在与LIOSAM框架设计到的知识点和工具较多，至少要熟悉ROS,gtsam,pcl几个库，算法层面需要熟悉点云匹配、IMU积分、因子图、三维转换等。对于熟悉SLAM的人来说可能较为简单上手，但是对于新入门的人来说则一开始会一头雾水。

基于上述这些原因，笔者进行了[SPL-LIO-SAM](Simple Lio Sam)项目，希望能够以最大幅度帮助初学者理解LIOSAM框架及里面的算法细节。本项目对LIOSAM做了如下改进

基于ROS2-humble实现

虽然LIOSAM源码中有一个ros2分支，但该分支的维护者并不是原作者，而且已经长时间没有人维护，且该分支存在一些bug没有被修复。为了能够在进行本项目的学习过程中熟悉最新的ROS2框架，笔者在LIOSAM的ros2分支基础上，修复了存在的bug，同时将功能较为独立的Transformfusion类抽取成独立的类。

为了便于上手及部署，笔者同时准备了适配的docker镜像供学习者使用。

简化

话题发布的简化

LIOSAM原始代码中有很多中间结果的发布，这些中间结果可以用来可视化及调试程序，但是对于学习者和开发者而言不仅意义不大，而且会在初始学习源码阶段误导学习者。本项目在话题发布上进行大幅度简化，简化后的设计如下：

简化后运行时的rqt_graph:

坐标系简化

LIOSAM中实用的坐标系并不复杂，但是作者提供的urdf包含了太多为了兼容性考虑的坐标系，这些坐标系对于实际运行和理解并没有用。因此，本项目基于ROS对坐标系的约束，将坐标系关系树简化为如下：

保留算法及定义完整性

虽然本项目去除了很多非必要话题，同时对于一些非必要代码也进行了简化，但对一些学习算法有帮助的细节依旧保留。比如激光里程计在发布的时候有mapping/odometry和mapping/odometry_incremental两个话题，两个话题虽然类似但是背后却有很不同的函数，但同时两者合一又不影响算法的运行。因此采取的做法时在代码中保留这部分代码，同时加以解释，但发布时只发布其中一个话题。

完善的注释及流程图

网上其实对LIOSAM源码的注释并不少，比如LIO-SAM-note和LIO-SAM-DetailedNote，还有一些blog也对源码做了解释，但是都不太完美，同时缺少流程性的组织。本项目借鉴了一些开源项目的注释，并加以完善，可以说，本项目是目前对LIOSAM注释最完善的开源项目。

为了最为清晰的展示LIOSAM算法不同模块的流程，该项目还对各个模块流程进行梳理，建立了完善的流程图设计

运行环境搭建

本项目提供基于ROS2-humble的镜像，并提供脚本可直接在镜像中启动可视化程序

数据

本项目同时提供转为rosbag2格式的数据包链接：https://pan.baidu.com/s/1hhHvn96uEsDYJNss3Z209Q 提取码：2478

export DATA_DIR=/path/to/download/ros2bag/dir

本项目中默认的配置文件params_default.yaml可以直接运行下面的数据：

park_dataset
walking_dataset
garden_dataset

环境部署

git clone git@github.com:zeal-up/Simple-LIO-SAM.git
cd Simple-LIO-SAM
./docker_run.sh -h  # show help message
./docker_run.sh -c /path/to/code/repo -d $DATA_DIR

# ./docker_into.sh  # enter the container next time

编译

cd Simple-LIO-SAM
./docker_into.sh

# 下面命令在镜像中执行
cd ~/ros_ws/
mkdir src && cd src &&ln -s /home/splsam/codes ./
cd ..
source /opt/ros/humble/setup.bash
colcon build --packages-select spl_lio_sam

运行

# docker镜像下运行
cd ~/ros_ws
source ./install/setup.bash
ros2 launch spl_lio_sam run.launch.py 

# 新开终端
cd Simple-LIO-SAM
./docker_into.sh
cd data/ros2/
ros2 bag play ./park_dataset/ --topics /points_raw /imu_raw

运行时画面：

开发环境搭建

由于本项目作者在使用vscode开发c++时经常会遇到无法快捷跳转问题（其实笔者发现有很多人在使用vscode开发时都会有这个问题），因此一并把解决方案写入项目中。

vscode开发cpp函数快捷跳转

安装cpp扩展 vscode侧边栏进入扩展选项（ctrl+shift+x)，搜索c++，安装C/C++ Extension Pack
打开项目

File->Open Folder

保存为工作区

File->Save Workspace as 选择目标目录及文件（默认以.code-workspace结尾）

打开刚才保存的工作区文件

File->Open File 选择刚才打开的工作区文件

对工作区添加c++库检索路径在刚才打开的工作区文件中，参考以下格式编辑

{
	"folders": [
		{
			"path": "../Simple-LIO-SAM"
		},
	],
	"settings": {
		"C_Cpp.default.includePath": [
			"${default}",
			"/usr/include/**",
			"/usr/include/c++/9/",
			"/usr/lib/gcc/x86_64-linux-gnu/8/include/",
			"/opt/ros/humble/include/**",

			"${workspaceFolder}/include",

            // 重点！！这里的路径要设置为ros2工作空间下spl_lio_sam编译出来的头文件
			"/path/to/ros2_ws/install/spl_lio_sam/include/spl_lio_sam"

		],
		"C_Cpp.files.exclude": {
			"**/.vscode": true,
			"**/.vs": true
		},
		"C_Cpp.default.cStandard": "c17",
		"C_Cpp.default.cppStandard": "c++17",
		"python.analysis.include": [
			"/opt/ros/humble/lib/python3.10/site-packages",
		],
		"python.autoComplete.extraPaths": [
			"/opt/ros/humble/lib/python3.10/site-packages",
		],
		"python.analysis.extraPaths": [
			"/opt/ros/humble/lib/python3.10/site-packages",
		],
	}
}

对工作区文件的解释

其实主要起作用的是工作区文件中settings->C_Cpp.default.includePath字段内容，vscode的cpp扩展会读取该工作空间文件下的这个字段，并将里面的路径进行解析加到头文件解析路径中。

这里有两个路径要注意的

"${workspaceFolder}/include"，也就是Simple-LIO-SAM仓库下的include路径
"/path/to/ros2_ws/install/spl_lio_sam/include/spl_lio_sam"，这个要设置成你的ros工作空间对应的路径

另外，如果发现自己路径设置完成，但是有些函数还提示下划线，要看看是不是那些函数是c++17或者更高的标准才支持的特性，在上面的配置文件中设置c++/c标准为17

其余注意

尽量不要用/path/**的格式添加include路径，虽然看起来很省事，但会影响检索效率
刚设置完路径需要等待vscode检索构建数据库，在vscode右下方状态条会提示正在进行检索
上面配置文件中"python.analysis.extraPaths","python.autoComplete.extraPaths","python.analysis.include"是设置python语法提示器的库检索路径，不同的python提示器插件会使用不同的路径。

zeal-up/Simple-LIO-SAM