LIO-SAM运行自己数据包遇到的问题解决--SLAM不学无数术小问题
本文详细介绍了如何使用LIO-SAM算法进行3D-SLAM建图，重点描述了硬件配置、数据格式要求、标定过程以及配置参数的修改。具体步骤包括使用速腾聚创激光雷达和RSLidar-SDK进行标定，以及配置IMU内参、外参和Lidar到IMU的外参。通过激光雷达和IMU的标定，实现了LIO-SAM算法的有效运行，并在室内环境中取得了较好的建图效果。摘要涵盖了核心内容，字数控制在100到200字之间，符合要求。
LIO-SAM 成功适配自己数据集
本文测试环境基于Ubuntu18.04 ROS melodic版本，硬件配置包括速腾聚创16线激光雷达（RS-Lidar-16）和超核电子CH110型9轴惯导（IMU），IMU采样频率设置为100Hz。需要注意的是，该教程主要用于指导安装和运行，但目前的版本可能已不再具有时效性。2022年7月下载的LIO-SAM源码的 frame_id 参数无需调整，主要工作内容集中在标定环节。建议在GitHub上寻找标定工具的替代方案。通过尝试不同的开源数据集，尤其是两种以上，可以更有效地进行标定。在标定过程中，建议根据实际需求进行参数调整和优化。
背景
近期对3D-SLAM算法的性能进行了深入研究，重点考察了具有优异性能的建图算法。在入门阶段，已经对LOAM、LeGo-LOAM、BLAM等算法进行了测试。这些算法均基于单激光雷达数据进行建图，建图效果虽有差异但变化不大。值得注意的是，融合激光雷达与IMU的性能表现出色，为此特意进行了深入研究。经过近两个月的实践探索，发现这一方向具有较大的发展潜力。在单激光雷达建图算法中，LIO-SAM的表现尤为突出，是目前见到效果最好的建图算法，也是个人测试时间最长、踩过坑最多的算法。基于个人实验数据，我总结了一些实用的建图算法应用经验，与各位分享交流。需要注意的是，其他类型的激光雷达与IMU设备，仅作为辅助参考。
一、数据格式要求
1.1IMU数据格式
虽然IMU的格式要求尚未完全探索，但采样频率确实需要注意。原作者采用了500Hz的IMU，并在源码中对其进行处理，最终转换为REP-105格式的数据。原作者建议用户最低使用200Hz的IMU，而笔者使用100Hz的IMU仍能达到类似效果，建议进行测试。
1.2激光雷达数据格式
LIO-SAM算法对激光雷达的数据格式有着严格的要求。以往的单激光雷达建图算法未充分考虑这一要求，通常只需满足XYZI格式（x, y, z, intensity）即可，但LIO-SAM要求更为严格，需采用XYZIRT格式（x, y, z, intensity, ring, timestamp），即算法内需使用激光雷达的通道数（ring参数）和时间戳（timestamp参数）。启动算法时会自动检查是否具备这两个参数。那么如何获取这两个参数呢？如果你使用的激光雷达与笔者相同，只需更新至RSLidar-SDK版本即可采集数据，具体操作可参考笔者另一篇教程：Ubuntu18.04安装速腾聚创最新驱动RSLidar_SDK以获取XYZIRT格式激光点云数据。然而，采集到的数据目前仍无法直接使用，速腾格式的点云与Velodyne格式的点云均存在不足。这又该如何解决呢？笔者在某技术论坛上找到了一位大佬提供的数据格式转换节点，该节点可将速腾格式的点云转换为Velodyne格式。具体安装步骤请参考笔者另一篇教程：速腾聚创雷达点云格式转换为Velodyne雷达点云格式。目前，其他雷达型号的数据我尚未获取，因此无法提供更多信息。
( 1.3GPS 数据格式要求)
作为一个小目标，我尚未尝试将GPS数据融入项目中。需要注意的是，GPS数据必须遵循sensor_msgs/NavSatFix这一特定的格式。由于GPS支持多种数据格式，原作者采用了该格式作为标准。如若有人成功测试其他格式，欢迎留言讨论。
作为一个小目标，我尚未尝试将GPS数据融入项目中。需要注意的是，GPS数据必须遵循sensor_msgs/NavSatFix这一特定的格式。由于GPS支持多种数据格式，原作者采用了该格式作为标准。如若有人成功测试其他格式，欢迎留言讨论。
1.4 坐标系的一致性校准（根据2022年7月后的源码，可能已不再需要进行该步骤；在建图过程中遇到困难时，可跳过该步骤）
简单来说，frame_id的设置有一定的要求。如果不打算修改源代码，可能需要在外部进行一些处理。其中涉及到内部坐标的映射关系，如果设置不一致，可能会导致ERROR，从而无法建立正确的图。原作提供的数据集中的frame_id设置遵循这一对应关系。
//激光雷达数据
/points_raw-----------frame_id:"velodyne"--------
//IMU数据
/imu_raw--------------frame_id:"imu-link"--------
//GPS数据
/gps/fix--------------frame_id:"navset_link"-----查看自己数据包的frame_id：
// 在播放数据包的时候使用如下指令查看某一Topic的frame_id：
rostopic echo /Topic | grep frame_id如果与原作的代码不同，则需要进行更正。具体修改方法可以在ROS官方文档的Wiki中获取，该工具包名为bag_tools。该工具包包含了许多关于数据包的操作工具，并附带了详细的使用教程，用户可以通过百度获取。举个例子，以下是常见的修改指令：
rosrun bag_tools change_frame_id.py -t /需要要改的topic -f 新的frame_id -i 旧.bag -o 新.bag二、lidar_align进行激光雷达与IMU外参标定
以上第一步仅解决了数据格式的处理问题，如何实现两种传感器的有效配合仍需进行更深入的标定工作，这一阶段的问题较为突出。我采用了瑞士苏黎世联邦理工大学自动驾驶实验室开发的lidar_align标定工具进行标定工作，不多赘述，感兴趣的朋友可以自行查阅相关资料。
2.1下载lidar_align
mkdir -p lidar_align/src
cd lidar_align/src
git clone https://github.com/ethz-asl/lidar_align.git
catkin_make首次编译会缺库报错
2.2安装nlopt库
sudo apt-get install libnlopt-dev将该配置文件复制至lidar_align/src/目录后进行二次编译。复制后运行编译器时，可能会出现错误信息。
2.3 处理定义冲突问题
//依次运行以下指令
sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak
sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak
sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h
sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h
catkin_make三次编译成功
2.4 改写IMU接口
该工具并非专为标定激光雷达与里程计设计，而是用于标定激光雷达与IMU。因此，需要对IMU接口进行改写以替代里程计接口。由此可见，该工具在精确标定激光雷达与里程计方面存在一定局限性，但其仍具有实用价值。改写接口的大佬尚未找到解决方案，导致无法正常连接。因此，建议删除该接口或与相关 parties 联系。以下是具体操作步骤：
- 打开loader.cpp文件
找到以下odom部分注释删掉都可
/* types.push_back(std::string("geometry_msgs/TransformStamped"));
rosbag::View view(bag, rosbag::TypeQuery(types));
size_t tform_num = 0;
for (const rosbag::MessageInstance& m : view) {
std::cout << " Loading transform: \e[1m" << tform_num++
<< "\e[0m from ros bag" << '\r' << std::flush;
geometry_msgs::TransformStamped transform_msg =
*(m.instantiate<geometry_msgs::TransformStamped>());
Timestamp stamp = transform_msg.header.stamp.sec * 1000000ll +
transform_msg.header.stamp.nsec / 1000ll;
Transform T(Transform::Translation(transform_msg.transform.translation.x,
transform_msg.transform.translation.y,
transform_msg.transform.translation.z),
Transform::Rotation(transform_msg.transform.rotation.w,
transform_msg.transform.rotation.x,
transform_msg.transform.rotation.y,
transform_msg.transform.rotation.z));
odom->addTransformData(stamp, T);
}
*/
将以上部分替换为：
types.push_back(std::string("sensor_msgs/Imu"));
rosbag::View view(bag, rosbag::TypeQuery(types));
size_t imu_num = 0;
double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;
ros::Time time;
double timeDiff,lastShiftX,lastShiftY,lastShiftZ;
for (const rosbag::MessageInstance& m : view){
std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush;
sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());
Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
if(imu_num==1){
time=imu.header.stamp;
Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));
odom->addTransformData(stamp, T);
}
else{
timeDiff=(imu.header.stamp-time).toSec();
time=imu.header.stamp;
velX=velX+imu.linear_acceleration.x*timeDiff;
velY=velX+imu.linear_acceleration.y*timeDiff;
velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;
lastShiftX=shiftX;
lastShiftY=shiftY;
lastShiftZ=shiftZ;
shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;
shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;
shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;
Transform T(Transform::Translation(shiftX,shiftY,shiftZ),
Transform::Rotation(imu.orientation.w,
imu.orientation.x,
imu.orientation.y,
imu.orientation.z));
odom->addTransformData(stamp, T);
}
}
并在开头添加头文件：
#include <sensor_msgs/Imu.h>第四次catkin_make编译。
2.5 录制数据以及标定
这一环节相对较为简单，只需同步采集激光雷达和IMU数据两分钟即可。建议将采集的数据量控制在2GB以内，若数据量超过2GB可能导致无法准确标定。采集两分钟的数据时，应尽量包含较大的旋转和平移量（即先直线行驶一段距离后再进行两次完整旋转），这样处理后，标定误差通常会收敛到几百个单位以下（实验者对数据包进行了初步标定，结果表明误差结果在17,000左右，供参考使用）。标定过程较为漫长，建议保持耐心。
2.5.1 修改launch文件
打开lidar_align.launch文件，将两分钟的数据包路经copy到下方：
改写了接口后，导入数据时无需使用表格形式，只需将数据包路径修改到launch文件中。以下是一些过程演示，标定时间可能需要一小时多一点，大约需要迭代300多次。
2.5.2启动launch开始标定迭代
source devel/setup.bash
roslaunch lidar_align lidar_align.launch
标定结束：
标定完成后，标定结果会在终端显示，同时生成相应的标定文件存放在result文件夹中。有了这个文件，就可以提取所需数据并写入LIO-SAM的配置参数文件。具体提取哪些数据将在后面详细说明。
三、imu_utils进行IMU内参标定
IMU内参是构建IMU模型的关键参数之一，在建图过程中，内参未标定往往会引发定位漂移（跑飞）现象。笔者采用港科大开发的标定工具imu_utils，通过Allan方差法对高斯白噪声和随机游走噪声（即零偏Bias）进行标定。完成内参标定后，即可着手进行IMU建图工作。
3.1下载imu_utils
在具体操作步骤中，我参考了大佬文章的内容。可以直接跳过，按照大佬的教程进行安装，随后在kalibr中完成相机和IMU的联合标定。
3.2准备标定需要的数据包
为确保数据质量，建议持续记录两个小时的IMU数据。请特别注意：在录制过程中，IMU必须保持静止状态，不允许有任何移动。确保在录制过程中，IMU的晃动幅度极小，建议仅采集Topic数据。无需担心内存占用，两小时的录制过程预计仅消耗约300MB的存储空间。建议将该数据包存储在imu_utils的工作目录中，与src目录保持一致。关于录制指令的详细说明，这里不做赘述，属于老手的常规操作。
3.3修改launch文件
在imu_utils目录下，存在多个launch文件，其中包含A3.launch文件。你可以将该文件复制，并将其重命名为test.launch。打开test.launch文件后，将其中的value字段替换为你的IMU数据topic。120表示数据采集的时间长度，持续两天即设置为120。
3.4开始标定
//启动launch
source devel/setup.bash
roslaunch imu_utils test.launch
//以两百倍速播放IMU数据包
rosbag play -r200 imu.bag当终端接收数据时并未产生反应，直到任务完成才会显示标定结果。此外，在data文件夹中会产生多个文件，其中我们关注的是第一个。
四、修改LIO-SAM配置参数
现在，我们终于到达了这个阶段，成功地获得了所有必要的参数配置。打开配置文件夹中的params.yaml文件，（建议备份一份源文件以供后续操作）。请注意，目前我们仅启用了激光雷达和IMU，而GPS未被配置。
4.1 修改订阅话题
在红框的地方修改雷达和IMU订阅话题
4.2修改IMU内参
在IMU设置部分，就是修改内参的地方。打开第三步标定好的IMU内参文件，文件路径为A3_imu_param.yaml，找到以下参数：
对应关系为：
// Acc误差模型高斯白噪声
imuAccNoise<---------->acc_n
// Gyro误差模型高斯白噪声
imuGyrNoise<----------> gyr_n
// Acc误差模型随机游走噪声
imuAccBiasN<---------->acc_w
// Gyro误差模型随机游走噪声
imuGyrBiasN<----------> gyr_w
imuAccNoise: 1.5126001818597128e-02
imuGyrNoise: 6.5980273374716312e-05
imuAccBiasN: 2.3794907799562417e-04
imuGyrBiasN: 8.9601374044261405e-07按照以上对应关系修改参数即可
4.3修改Lidar–>IMU外参
启动下一步标定激光雷达与IMU的外参配置的calibration_oprater_hp_5402_590670726118389616.txt文件，定位所需参数。
将上述方框标注的参数对应写到下方配置文件中：
extrinsicTrans: [0.00290376, -0.00715269, 0.0395843]
extrinsicRot: [-0.975156 ,-0.221516 ,-0.000975888,
0.221498 ,-0.975118 , 0.00911853,
-0.00297151 ,0.00867584 ,0.999958]
extrinsicRPY: [-0.975156 ,-0.221516 ,-0.000975888,
0.221498 ,-0.975118 , 0.00911853,
-0.00297151 ,0.00867584 ,0.999958]其他参数建议保持不变，修改后及时保存。建议在编译完成后，没有错误的情况下运行launch文件启动播包。此外，默认情况下不会保存pcd文件，用户可以自行调整，但每次运行前需修改路径，否则会覆盖上次生成的文件。
五、起飞
这一系列操作逐一执行起来确实颇为费力。听听歌曲放松心情，起飞前做好准备 芜湖起飞.
source devel/setup.bash
roslaunch lio_sam run.launch启动之后可以播包试试了
下面是笔者自己数据包建图效果（室内）：
保存到本地的pcd文件
六、总结
本文仅介绍了融合速腾激光雷达与IMU方案的具体实施步骤，并未展开理论分析，下面将列举一些参考文献。
建图效果读者自评，参数优化程度尚待提升，仅进行粗略标定。值得注意的是，激光雷达与IMU的外参定位精度未达理想状态，但相较于仅采用激光雷达的紧耦合算法，该方法较其他单激光雷达传感器算法在效果上已有显著提升。自发现LIO-SAM建图效果以来，逐步适配并测试自己的数据集，直至获得上述效果，大约两个月左右。作为3D-SLAM学习者，上述结果仅反映个人建图过程中的问题与解决方法，期待进一步探讨更优解决方案。
以下是笔者参考过的文章，感谢各位大佬分享：
安装教程参考：
LIO-SAM：环境配置、测试安装、数据集适配
各种知识点参考:
- 激光SLAM在运行LIO-SAM过程中可能存在的潜在问题
3.Lidar与IMU标定代码实战：lidar_align.
5.LIO-SAM论文翻译.
第6步，通过imu_utils对IMU进行校准，随后将其应用于kalibr软件中，用于相机与IMU的综合校准。
7.IMU误差模型和校准.
第9项，激光雷达与IMU协同标定并运行LIOSAM算法
版权声明信息：本文为个人原创文章，遵循CC 4.0 BY-SA授权协议。如需转载，请附上原文链接及版权声明说明。访问该文章的详细信息，请访问：