作者 | Zeal 编辑 | 汽车人
原文链接：https://www.zhihu.com/column/c_1619085291536433152
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
点击进入→自动驾驶之心【SLAM】技术交流群
后台回复【SLAM综述】获取视觉SLAM、激光SLAM、RGBD-SLAM等多篇综述！
LIOSAM介绍
[LIOSAM]（Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping），紧耦合激光惯性里程计方法。作者[TixiaoShan]是SLAM届大神，在[LIOSAM]之前发表过[LeGO-LOAM]，在[LIOSAM]之后发布了[LVI-SAM]——在[LIOSAM]基础上又加入了语义定位因子。网上关于[LIOSAM]的解读已经有很多，这里不会进行太过详细的复述，具体的内容解析会放在后面系列文章慢慢仔细阐述。
简单来说，[LIOSAM]整体的框架与其他方法区别在与：
使用因子图作为后端优化框架，将IMU预积分与雷达点云匹配及回环检测等紧耦合
使用当前帧-局部地图匹配而不是对全局地图匹配来加速点云匹配速度
基于位置关系选择回环帧，并使用点云匹配计算回环因子
使用高频IMU并进行简单积分对点云做运动畸变校正
另外，LIOSAM作者的工程功底很好，LIOSAM代码库被github上很多SLAM开源项目作为code base
本项目目的
[LIOSAM]源代码虽然不能说庞大，甚至可以说简单，因为整个代码库主要只要5个文件。
但是原代码里面每个模块都通过ROS的topic与其他模块有紧密的联系，且非常多的冗余代码和topic，这导致了 整个的流程对新手十分不友好，下图是LIOSAM原本运行时的rqt_graph
第二个复杂之处在于[LIOSAM]框架涉及到的知识点和工具较多，至少要熟悉ROS,gtsam,pcl几个库，算法层面需要熟悉点云匹配、IMU积分、因子图、三维转换等。对于熟悉SLAM的人来说可能较为简单上手，但是对于新入门的人来说则一开始会一头雾水。
基于上述这些原因，笔者基于[LIOSAM]改进，完成了SPL-LIO-SAM项目，希望能够以最大幅度帮助初学者理解[LIOSAM]框架及里面的算法细节。
适用人群
如果你是SLAM大牛，那本项目可能对你没有用处。如果你是以下人群之一，笔者相信本项目可以让你受益匪浅。
SLAM初学者。本项目对代码注释和讲解可以说细致到令人发指，因此十分适合初学者学习。
接触过SLAM，希望将SLAM算法工程化，并加以实现。
SLAM学习者或工程师，但是希望对[LOAM],[LeGO-LOAM],[LVI-SAM],[loam_velodyne]其中任何一种算法进行精细学习
Simple-LIO-SAM项目特点/与LIOSAM区别
基于最新的ROS2发行版humble实现
虽然[LIOSAM]源码中有一个ros2分支，但该分支的维护者并不是原作者，而且已经长时间没有人维护，且该分支存在一些bug没有被修复。为了能够在进行本项目的学习过程中 熟悉最新的ROS2框架，笔者在LIOSAM的ros2分支基础上，修复了存在的bug，同时将功能较为独立的Transformfusion类抽取成独立的类。
开箱即用docker开发环境
为了便于上手及部署，笔者同时准备了适配的docker镜像供学习者使用。
简化
话题发布的简化
[LIOSAM]原始代码中有很多中间结果的发布，这些中间结果可以用来可视化及调试程序，但是对于学习者和开发者而言不仅意义不大，而且会在初始学习源码阶段误导学习者。本项目在话题发布上进行大幅度简化，简化后的设计如下：
简化后运行时的rqt_graph:
坐标系简化
[LIOSAM]中实用的坐标系并不复杂，但是作者提供的urdf包含了太多为了兼容性考虑的坐标系，这些坐标系对于实际运行和理解并没有用。因此，本项目基于ROS对坐标系的约束，将 坐标系关系树简化为如下：
保留算法及定义完整性
虽然本项目去除了很多非必要topic，同时对于一些非必要代码也进行了简化，但对一些学习算法有帮助的细节依旧保留。比如激光里程计在发布的时候有mapping/odometry和mapping/odometry_incremental两个话题，这两个话题虽然类似但是背后却有很不同的含义，但同时两者合一又不影响算法的运行。因此采取的做法时在代码中保留这部分代码，同时加以解释，但发布时只发布其中一个话题。
完善的注释及流程图
网上对[LIOSAM]源码的注释其实并不少，比如[LIO-SAM-note]和[LIO-SAM-DetailedNote]，还有一些blog也对源码做了解释，但是都不太完美，同时缺少项目性的组织。本项目借鉴了一些开源项目的注释，并加以完善，可以说，本项目是目前对[LIOSAM]注释最完善的开源项目。Simple-LIO-SAM代码
为了最为清晰的展示[LIOSAM]算法不同模块的流程，该项目还对各个模块流程进行梳理，建立了完善的流程图设计
运行环境搭建
本项目提供基于ROS2-humble的镜像，并提供脚本可直接在镜像中启动可视化程序。docker镜像部署见下文。
数据
本项目同时提供转为rosbag2格式的数据包，下载地址:暂时未上传
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提示器插件会使用不同的路径。
总体流程
话不多说，直接上干货。下图是整理出来的[LIOSAM]整体运行流程
整体认识
所谓的紧耦合，笔者是这么理解的：IMU本身就可以积分出来一个位姿，连续帧的点云匹配也可以计算出一个位姿，如果两者分别进行，然后再做一个加权融合，那么就属于松耦合。而从上面的框架图也可以看出，[LIOSAM]的融合框架整体呈现一个圈，每个环节都为下一个环节提供信息，最后一个环节又为第一个环节提供信息。同时，耦合之处还在于以下几点：
高频的IMU信息对激光雷达点云做了畸变矫正，做了畸变矫正后的点云被用来做匹配计算激光里程计
IMU的积分结果被用来当作该帧点云的初值，为点云匹配提供了较好的初始估计
雷达里程计的结果被用来计算IMU的偏差，同时也被用来持续纠正IMU里程计
虽然[LIOSAM]框架中还可以融合GPS信息，但是主要还是对雷达点云和IMU信息做融合，因此，可以认为输入是点云和IMU数据，输出是小车当前时刻的位姿，以及对历史时刻位姿的平滑。
框架的入口是点云去畸变模块，主要的功能就是对激光雷达运动畸变做矫正，然后输出去完畸变的点云。
去完畸变的点云经过特征提取模块，提取出角特征点和平面特征点，被图优化模块中的点云匹配部分用来做点云匹配。
图优化模块利用去完畸变后的点云与历史帧构建的局部地图做匹配，得出较准确的里程计结果。同时利用图优化，加入回环检测因子，使得整体估计更为准确顺滑
IMU预积分模块从IMU和图优化模块接收数据，对IMU的原始数据做积分，得出每一帧点云的初始位姿，提供给图优化模块作为优化初始值。同时又从图优化模块拿出经过点云、图优化校正后的里程计结果校正本身的结果
坐标系发布主要是为了
RIVZ显示中用的，主要就是将各种里程计结果转换成TF坐标关系发布出去。在原始[LIOSAM]中是与imuPreintegration放置在同一个文件
运动畸变矫正
对应博文：Simple-LIO-SAM——（五）点云去畸变模块
为什么激光雷达有运动畸变？
激光雷达通过发射高能量激光线束，计算激光线束往返时间，得到该激光线打到的物体的距离。以机械式激光雷达为例，雷达在统一时刻可以发送多条激光线（32、64、128），这多条激光线在垂直方向上可以扫到三维空间中垂直的一条线，激光雷达通过环绕一周的方式可以对360的空间做三维成像
由于激光雷达是通过激光线束环绕一周的方式成像一帧的点云，那么这一帧点云的点就是在不同的时间探测的。如果激光线束在环绕一周的过程中，激光雷达是在运动的，那么这一帧点云就由于激光雷达的运动而变得不准确，因此需要对其做运动畸变校正。
（上面这幅图片引用自livox官网[Liovox_distortion_removal]）
激光雷达输出的每一帧点云，里面的每一个点，都有一个属性，记录了该点距离该帧点云起始时刻的时间间隔，也就是下面代码段里的time字段。
// imageProjection.cpp 49-60
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring) (float, time, time)
)由于IMU是非常高频的，因此我们可以得出该帧点云成像时间段内的激光雷达位移和旋转的变换，然后通过每一个点的时间，可以根据时间进行插值得出每一个点相对于起始点的转换关系。（注：LIOSAM里面是直接通过时间戳查找，而不是进行插值）。从而，我们就可以将每个点都转换到起始点的坐标系
点云特征提取
对应博文：Simple-LIO-SAM——（六）特征提取模块
激光里程计中一般会涉及到点云匹配，如果用原始点云进行匹配由于点数过多的关系，会导致耗时太长，因此[LIOSAM]中用类似[LeGO-LOAM]的方法进行边缘点,平面点两类特征点的提取，后面点云匹配部分使用这两种特征点进行高斯-牛顿法求解出匹配关系。
这部分的代码相对简单，后面关于这个模块的详细内容会介绍，这里唯一要清楚的是，这里的点云特征点提取用的是每个点的曲率，选择曲率大的点作为边缘点，选择曲率小的点作为平面点。但是曲率的计算是用每个点周围的点到其平均距离作为近似。
IMU预积分模块
对应博文：Simple-LIO-SAM——（八）IMU预积分模块
这一个模块虽然从原理和实现上来说稍微有一点点复杂，但是从功能上简单来说只有一个功能，就是实时对IMU原始数据做处理（积分），得到每一个时刻的IMU里程计（也是6自由度位姿），然后该位姿被用来作为该帧点云的初始估计位姿，这个初始估计位姿会被用来作为点云匹配的初始估计。
点云匹配及图优化
对应博文：
Simple-LIO-SAM——（九）点云匹配算法详解
Simple-LIO-SAM——（十）后端优化模块详解
这是整个框架最复杂的一个模块（也许是没有很好解耦？）。这个模块主要包含了以下几个功能：
点云匹配
回环检测
因子图构建及优化
根据关键帧信息构建全局地图
具体内容会在后面的章节展开，这里应该要有几个感性认识：
一般点云精细匹配（比如ICP算法）需要有一个良好的初始值，这里的初始值可以理解成采用IMU预积分模块的输出。
点云匹配部分用的是沿用自LOAM->LeGO-LOAM的点云匹配算法，不是使用ICP。[LeGO-LOAM]也是[TixiaoShan]在[LIOSAM]之前的一篇相关论文
回环检测根据里程计的x,y,z检索紧邻帧，然后通过ICP算法计算当前帧与近邻帧的转换关系
经过点云匹配后的位姿会经过图优化得出更平滑的位姿
地图和因子图的因子实际上是以关键帧的形态存在后端优化部分，关键帧的选取会比较稀疏，比如距离小与1米不会采用为关键帧。不采用为关键帧的点云会执行完点云匹配后就直接输出位姿。
在有新的回环因子加入因子图后，会进行多次因子图优化器的更新步骤，然后将所有历史关键帧的位姿都更新一遍
这个部分还支持
GPS里程计的监听，并将GPS里程计也加入因子图进行优化
坐标系规定
坐标系规定
原有的urdf文件加入了太多额外没必要的关节，导致rviz中tf-tree关系太过复杂。为了简化坐标系关系，但同时又维持坐标系关系标准化并且与ROS的规定相同（ROS中关于姿态估计的坐标系关系主要参考：[robot_localization_coordinate_system]，这里对项目中的坐标系关系做了一些简化修改。根据robot_localization包的规定，所有姿态都要转到世界坐标系，通常是map或者odom坐标系。所有的twist数据，都要转到base_link坐标系下处理。
Simple-LIO-SAM坐标系说明
根据REP资料，一般有4个坐标系
earth,map,odom,base_link。由于在SLAM中一般没有引入地球坐标系。所以在这里只有后三个坐标系。同时，如果是以起始点作为地图原点，map,odom两个坐标系是重合的关系。另外，为了可以在Rivz显示原始点云信息，则也加入一个雷达坐标系。
为了让坐标系定义自洽，虽然在算法中没有直接用到，但是IMU加速度速度和角速度应该定义在IMU坐标系，因此引入IMU坐标系
为了让定义更为标准，这里保留
map坐标系，虽然在SLAM中map,odom大部分情况是重叠的因此，整个spl_lio_sam框架可以简化成5个坐标系：
map,odom,base_link,lidar_link,imu_link坐标系。其中，map是地图原点，在建图时是起始点，在重定位模式时，是地图原点。odom是里程计坐标系；base_link是车辆自身坐标系。lidar_link坐标系是雷达坐标系， 在lio_sam中是与base_link坐标系对齐的，但是lidar_link字段会根据雷达型号不同，被修改。imu_link坐标系是IMU坐标系，配置文件中 有指明IMU坐标系到Lidar坐标系的转换关系。
坐标系关系的其他说明
ROS的Message Header只有一个frame_id字段，表明这个数据是在哪个坐标系下的。
nav_msgs/Odometry中还有一个child_frame_id字段，nav_msgs/Odometry中的twist数据应该是在child_frame_id坐标系下的数据
The twist in this message should be specified in the coordinate frame given by the child_frame_id
utiliti.cpp文件
这是[LIOSAM]里一个最基础的文件，主要有几个重要功能：
作为所有其他节点的基类，放置所有从配置文件读取的配置字
提供一些实用函数
配置[QoS]
本文对这个文件的详细内容和重点详细解读。
配置基类
[LIOSAM]为了方便配置，把所有节点的配置项都写在同一个文件Simple-LIO-SAM/config/params.yaml，同时构建了一个基类ParamServer统一读取该配置文件，其余模块都从该基类继承。
这可能不是最优的方式，不过从实现上来说的确是最简单的。
每个模块的具体配置项后面具体讲解到会逐一解释，目前只需要知道是在哪里配置就行。
// Topics
string pointCloudTopic; // 原始点云数据话题（/points_raw）
string imuTopic; // 原始IMU数据话题（/imu_correct）
string imuOdomTopic; // IMU里程计，在imuPreintegration中对IMU做预积分得到（/lio_sam/imu/odometry）
string lidarOdomTopic; // 雷达里程计，在mapOptimization中得到（/lio_sam/mapping/odometry）
string gpsTopic; // 原始gps经过robot_localization包计算得到，暂未使用
// Services
string saveMapSrv; // 保存地图service地址
// Frames
string imuFrame; // IMU数据坐标系，如果IMU和激光雷达坐标系硬件对齐，可以认为IMU、Lidar、Chassis坐标系相同
string lidarFrame; // 激光雷达坐标系，点云数据坐标系，由激光雷达发布的数据指定。与lidarFrame相同，但是不同雷达有不同的名称
string baseLinkFrame; // 车辆底盘坐标系
string odomFrame; // 地图坐标系，在SLAM中一般也是世界坐标系，通常是车辆的起始坐标系
// GPS Settings
bool useImuHeadingInitialization;
bool useGpsElevation;
float gpsCovThreshold;
float poseCovThreshold;
// Save pcd
bool savePCD;
string savePCDDirectory;
// Lidar Sensor Configuration
SensorType sensor;
int N_SCAN;
int Horizon_SCAN;
int downsampleRate;
float lidarMinRange;
float lidarMaxRange;
// IMU
float imuAccNoise; // IMU加速度噪声协方差，可以用Allen方差标定；这里三个轴设为相同的方差
float imuGyrNoise; // IMU角速度噪声协方差，可以用Allen方差标定；这里三个轴设为相同的方差
float imuAccBiasN; // IMU加速度偏差，三轴统一
float imuGyrBiasN; // IMU角速度偏差，三轴统一
float imuGravity; // 重力加速度值
float imuRPYWeight; // 算法中使用IMU的roll、pitch角对激光里程计的结果加权融合
vector<double> extRotV; // IMU加速度向量到雷达坐标系的旋转
vector<double> extRPYV; // IMU角速度向量到雷达坐标系的旋转
vector<double> extTransV; // IMU向量到雷达坐标系的平移：P_{lidar} = T * P_{imu}
Eigen::Matrix3d extRot; // IMU加速度向量到雷达坐标系的旋转
Eigen::Matrix3d extRPY; // IMU角速度向量到雷达坐标系的旋转
Eigen::Vector3d extTrans; // IMU向量到雷达坐标系的平移：P_{lidar} = T * P_{imu}
Eigen::Quaterniond extQRPY; // IMU角速度向量到雷达坐标系的旋转（四元数形式）
// LOAM
float edgeThreshold; // 边缘特征点提取阈值
float surfThreshold; // 平面特征点提取阈值
int edgeFeatureMinValidNum; // 边缘特征点数量阈值（default:10)
int surfFeatureMinValidNum; // 平面特征点数量阈值（default:100)
// voxel filter paprams
float odometrySurfLeafSize;
float mappingCornerLeafSize;
float mappingSurfLeafSize ;
float z_tollerance; // 限制z轴平移的大小
float rotation_tollerance; // 限制roll、pitch角的大小
// CPU Params
int numberOfCores; // 在点云匹配中使用指令集并行加速（default:4）
double mappingProcessInterval; // 点云帧处理时间间隔（default:0.15s）
// Surrounding map
float surroundingkeyframeAddingDistThreshold; // 当前帧需要与上一帧距离大于1米或者角度大于0.2度才有可能采纳为关键帧
float surroundingkeyframeAddingAngleThreshold; // 当前帧需要与上一帧距离大于1米或者角度大于0.2度才有可能采纳为关键帧
float surroundingKeyframeDensity; // 构建局部地图时对采用的关键帧数量做降采样
float surroundingKeyframeSearchRadius; // 构建局部地图时关键帧的检索半径
// Loop closure
bool loopClosureEnableFlag;
float loopClosureFrequency; // 回环检测独立线程的执行频率
int surroundingKeyframeSize; // 回环检测构建局部地图的最大关键帧数量
float historyKeyframeSearchRadius; // 执行回环检测时关键帧的检索半径
float historyKeyframeSearchTimeDiff; // 执行回环检测时关键帧的检索时间范围
int historyKeyframeSearchNum; // 执行回环检测时融合局部地图时对目标关键帧执行+-25帧的关键帧融合
float historyKeyframeFitnessScore; // 执行回环检测时使用ICP做点云匹配，阈值大于0.3认为匹配失败，不采纳当前回环检测
// global map visualization radius
float globalMapVisualizationSearchRadius;
float globalMapVisualizationPoseDensity;
float globalMapVisualizationLeafSize;实用函数
这里主要讲解其中一个imuConverter函数。该函数的功能主要是将IMU的原始数据旋转到Lidar坐标系。这里要先知道，在[LIOSAM]中， 默认将雷达坐标系和小车的坐标系等同为同一个坐标系，同时，这里只做了旋转操作，没有做平移操作。代码中有很多处位置都用到这个函数，因此需要理解这个函数到底做了什么。简单来说这个函数就是做了一个向量坐标变换。旋转矩阵操作空间中一个点有两种意义：1）三维旋转；2）坐标变换。这两种不同的操作在做连续的操作时，对应旋转矩阵的左乘和右乘。由于笔者以前的研究方向是六自由度姿态估计，用三维旋转操作比较多，一开始被这里的矩阵右乘搞蒙了。关于旋转矩阵的左乘和右乘，可以看这篇博客
这个函数的代码和解释如下
/*
将原始IMU数据：三轴加速度、三轴角速度、三轴角度，与雷达坐标系进行旋转对齐
+ 对齐之后输出的加速度、角速度、角度的x，y，z就变成雷达坐标系的x，y，z
+ 这里的特殊之处在于允许IMU的加速度、角速度与角度的输出是两个不同的坐标系。但在算法中，角度的输出除了用来做第一帧的初始化和加权融合，似乎没有其他作用
+ 这里是将IMU的三个轴与雷达的三个轴在旋转上做对齐，不能加上平移
+ 对向量做坐标系变换，对多个变换的复合应该是右乘
*/
sensor_msgs::msg::Imu imuConverter(const sensor_msgs::msg::Imu& imu_in)
{
sensor_msgs::msg::Imu imu_out = imu_in;
/*
对加速度向量做坐标系变换，注意这里要理解成坐标系变换，也就是同一个加速度在IMU坐标系和Lidar坐标系的不同表达。不能想象成对加速度做旋转
*/
Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
acc = extRot * acc;
imu_out.linear_acceleration.x = acc.x();
imu_out.linear_acceleration.y = acc.y();
imu_out.linear_acceleration.z = acc.z();
/*
对角速度做坐标系变换。将IMU坐标系下的向量变换到雷达坐标系。
*/
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
gyr = extRot * gyr;
imu_out.angular_velocity.x = gyr.x();
imu_out.angular_velocity.y = gyr.y();
imu_out.angular_velocity.z = gyr.z();
/*
对角度做坐标系变换。
+ q_from是IMU在全局坐标系下的位姿，q_from: transformation_from_map_to_imu
+ extQRPY如果与extRot对应的话应该是lidar到imu的变换：transformation_from_lidar_to_imu
+ q_final是将雷达点云从雷达坐标系转换到map坐标系的变换，也是：transformation_from_map_to_lidar -> pcd_in_map = q_final * pcd_in_lidar
+ 这里原代码是q_final = q_from * extQRPY；似乎有点问题，还是按照我的推导修改成q_final = q_from * extQRPT.inverse()；由于这里的extQRPY是
+ 直接从配置文件里面读取的，所以这里加不加逆只需要在配置文件里改就行。认为这里有问题的假设是认为extQRPY和extRot的坐标系关系的定义是一致的，也就是
+ 将imu坐标系下的向量转换到雷达坐标系下。如果作者对这两者的定义刚好是相反的，那这里就没有问题。
*/
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
Eigen::Quaterniond q_final = q_from * extQRPY.inverse();
imu_out.orientation.x = q_final.x();
imu_out.orientation.y = q_final.y();
imu_out.orientation.z = q_final.z();
imu_out.orientation.w = q_final.w();
if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
{
RCLCPP_ERROR(get_logger(), "Invalid quaternion, please use a 9-axis IMU!");
rclcpp::shutdown();
}
return imu_out;
}QoS:Quality of Service
关于ROS2中[QoS]的基本介绍，参考官方文档学习。[QoS]的设置中，有两个参数是最重要的。一个是depth，一个是RELIABILITY。depth可以简要理解成中间件DDS为这个消息预留的缓存队列长度。RELIABILITY主要有两种选项best_effort和reliable，它们的关系类似TCP,UDP。best_effort不能保证每一个消息到到达接收端，但可以保持最好的实时信。reliable保证每一帧消息 都到达接收端，但是无法保证实时性。ROS2中专门为传感器预设了一个SensorDataQoS，里面采用的就是best_effort的设置。
在[LIOSAM]设置了三种QoS，分别为：原始IMU数据、原始雷达数据、框架内部传输
原始IMU数据QoS
/// @brief 原始IMU数据的QOS，因为IMU数据较小，所以depth可以设成较大
rmw_qos_profile_t qos_profile_imu{
RMW_QOS_POLICY_HISTORY_KEEP_LAST,
2000,
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
RMW_QOS_POLICY_DURABILITY_VOLATILE,
RMW_QOS_DEADLINE_DEFAULT,
RMW_QOS_LIFESPAN_DEFAULT,
RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
false
};
auto qos_imu = rclcpp::QoS(
rclcpp::QoSInitialization(
qos_profile_imu.history,
qos_profile_imu.depth
),
qos_profile_imu);原始雷达数据QoS
/// @brief 原始雷达数据topic的QOS，主要是best_effort和depth起作用
rmw_qos_profile_t qos_profile_lidar{
RMW_QOS_POLICY_HISTORY_KEEP_LAST,
5,
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
RMW_QOS_POLICY_DURABILITY_VOLATILE,
RMW_QOS_DEADLINE_DEFAULT,
RMW_QOS_LIFESPAN_DEFAULT,
RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
false
};
auto qos_lidar = rclcpp::QoS(
rclcpp::QoSInitialization(
qos_profile_lidar.history,
qos_profile_lidar.depth
),
qos_profile_lidar);算法框架内部传输QoS
算法框架内部要求有最高的实时性，并且由于传输中有很多点云类型的数据（较大），因此作者把depth设置为1
/// @brief 算法框架中默认的QOS，主要是depth=1和reliability=best_effort起作用。对于传输实时性有要求，不要求每个数据可接收的消息，一般
/// 设成best_effort。在ROS2中对于传感器数据，有一个内置的QOS叫rclcpp::SensorDataQoS()
rmw_qos_profile_t qos_profile{
RMW_QOS_POLICY_HISTORY_KEEP_LAST,
1,
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
RMW_QOS_POLICY_DURABILITY_VOLATILE,
RMW_QOS_DEADLINE_DEFAULT,
RMW_QOS_LIFESPAN_DEFAULT,
RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
false
};
auto qos = rclcpp::QoS(
rclcpp::QoSInitialization(
qos_profile.history,
qos_profile.depth
),
qos_profile);点云去畸变概览
点云去畸变模块（imageProjection.cpp文件，文件命名应该是继承了[LeGO-LOAM]，其实我觉得应该叫做数据预处理模块更合适）主要有以下几个作用
去除激光雷达点云的运动畸变
检索每一帧点云对应的IMU输出角度、IMU里程计位姿（发布自ImuPreintegration），以便MapOptimization模块用这些信息作为该帧点云的初始位姿
转换点云格式，计算
Range字段，方便特征提取模块进行边缘点、平面点提取
从总体流程图不难看出，点云去畸变模块是整个算法框架的入口。点云去畸变模块的总体流程如下图：
消息订阅及发布
从上图可以看到，这个模块订阅点云和IMU的原始数据，输出一个以CloudInfo格式的topic。下面详细讲解CloudInfo这个数据格式。
CloudInfo数据类型
经过去畸变的点云通过算法自定义的数据格式Simple-LIO-SAM/msg/CloudInfo.msg发布；特征提取模块也是将特征点放入这个自定义数据格式发布。
# Cloud Info
std_msgs/Header header
# 在进行点云去畸变时，把range数据展开成一维向量
# ring代表第几条激光线数，比如16线的激光雷达有16个ring
# start_ring_index记录第一个ring在一维向量中的起始索引
# end_ring_index记录第一个ring在一维向量中的结束索引
int32[] start_ring_index
int32[] end_ring_index
# 记录一维的Range数据中每一个点在原始range图片中属于那一个列
int32[] point_col_ind # point column index in range image
# 所有点的range数值，展开为一维数组
float32[] point_range # point range
int64 imu_available
int64 odom_available
# Attitude for LOAM initialization
# 从IMU原始数据获取的roll、pitch、yaw角初值
# 当上面的imu_avaliable为true，odom_available为false的时候，会被用来
# 当作这一帧点云的初值
float32 imu_roll_init
float32 imu_pitch_init
float32 imu_yaw_init
# Initial guess from imu pre-integration
# 从imuPreintegration，IMU预积分模块获取的与这一帧雷达最接近的初始位姿
# 当上面的odom_available标签被置为true的时候会被使用
# 优先级比imu_xxx_init高
float32 initial_guess_x
float32 initial_guess_y
float32 initial_guess_z
float32 initial_guess_roll
float32 initial_guess_pitch
float32 initial_guess_yaw
# Point cloud messages
# 去畸变后的原始点云
# 在经过imageProjection模块后，这个数据字段被填充。在经过特征提取之后这个字段被清空
sensor_msgs/PointCloud2 cloud_deskewed
# featureExtraction模块提取出的点云边缘点
sensor_msgs/PointCloud2 cloud_corner
# featureExtraction模块提取出的点云平面点
sensor_msgs/PointCloud2 cloud_surface这里主要是前面几个数组类型会比较难理解，主要是要知道在去畸变模块中，将所有点的range信息（也就是点距离雷达的距离）拉成一个一维的数组，即float32[] point_range。同时，cloud_deskewed里存储的是去完畸变后的点云，里面点的顺序与point_range的顺序是一样的。
cloud_corner和cloud_surface两个字段在imageProjection模块中没有被放置数据。特征提取模块会将边缘点、平面点放置在这两个字段中，同时将cloud_deskewed字段清空。
功能解读
IMU及IMU里程计处理
ImageProjection监听IMU原始数据与从ImuPreintegration（后面会详细讲解这个模块）发布出来的IMU里程计数据，分别由两个回调函数处理，对于IMU原始数据，只是简单地将其旋转到雷达坐标系后塞入缓存队列。对于IMU历程计，则是直接塞入缓存队列。
去畸变流程
一句话说明点云运动畸变校正：根据每一帧点云中每一个点的时间戳，计算该点到该帧点云起始点的旋转平移变换，将每一个点变换到起始点的坐标系。
要注意的是，算法中对运动畸变校正只做了旋转校正，没有做平移校正。
去畸变流程发生在点云的回调函数中
// imageProjection::cloudHandler
/** 原始雷达点云话题的回调函数，实际上真正做点云处理的函数
* 实际处理流程是单线程流水线式处理，这个函数后面的所有函数都是为这个函数服务，因此需要了解
* 点云去畸变的流程。
* 订阅原始lidar数据
* 1、转换点云为统一格式，提取点云信息
* 1）添加一帧激光点云到队列，取出最早一帧作为当前帧
* 2) 计算起止时间戳，检查数据有效性
* 2、从IMU数据和IMU里程计数据中提取去畸变信息
* imu数据：
* 1) 遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
* 2) 用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
* imu里程计数据：
* 1) 遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
* 2) 用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
* 3、当前帧激光点云运动畸变校正
* 1) 检查激光点距离、扫描线是否合规
* 2) 激光运动畸变校正，保存激光点
* 4、提取有效激光点，集合信息到准备发布的cloud_info数据包
* 5、发布当前帧校正后点云，有效点和其他信息
* 6、重置参数，接收每帧lidar数据都要重置这些参数
**/
void cloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudMsg)
{
// 1、提取、转换点云为统一格式
if (!cachePointCloud(laserCloudMsg))
return;
// 2、从IMU数据和IMU里程计数据中提取去畸变信息
if (!deskewInfo())
return;
// 3、当前帧激光点云运动畸变校正
projectPointCloud();
// 4、提取有效激光点，集合信息到准备发布的cloud_info数据包
cloudExtraction();
// 5、发布当前帧校正后点云，有效点和其他信息
publishClouds();
// 6、重置参数，接收每帧lidar数据都要重置这些参数
resetParameters();
}提取、转换点云为统一格式
提取去畸变信息
这里有几点要注意
从IMU原始数据计算每一时刻的位姿变换用的是近似算法（因为一帧点云成像时间很短，一般小于100ms）
// 对角度做积分
// 再次强调，对角速度的积分不是简单的角速度乘以间隔时间
// 关于角速度的积分公式可以查阅：https://zhuanlan.zhihu.com/p/591613108
static double timeDiff;
timeDiff = currentImuTime - imuTime[imuPointerCur-1];
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime;
++imuPointerCur;对于速度较低，角度变化不那么剧烈的行驶系统，即使不做点云畸变校正也没有问题
对点云做去畸变处理
提取有效点云并集合其他信息
这里主要是将经过去畸变处理后记录下来的有效点进行数据转换，并把各种信息填入准备发布的cloud_info消息中。
发布点云
视频课程来了！
自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、传感器标定、传感器部署、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知、决策规划、轨迹预测等多个方向学习视频，欢迎大家自取（扫码进入学习）
（扫码学习最新视频）
国内首个自动驾驶学习社区
近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！
【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；
添加汽车人助理微信邀请入群
备注：学校/公司+方向+昵称
4011
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
