LIO-SAM运行自己数据包遇到的问题解决--SLAM不学无数术小问题

LIO-SAM 成功适配自己数据集

注意本文测试环境 Ubuntu18.04 ROS melodic版本
笔者用到的硬件以简单参数：
激光雷达：速腾聚创16线激光雷达（RS-Lidar-16）
IMU：超核电子CH110型 9轴惯导，使用频率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参数和时间戳timestep参数，启动算法时会检查是否具有这两个参数。那么如何获得这两个参数呢？ 如果你同笔者使用的激光雷达一样，你可以将激光雷达的驱动更新到RSLidar-SDK版本即可采集，参考笔者另一篇教程安装速腾最新的驱动：Ubuntu18.04 安装速腾聚创最新驱动RSLidar_SDK采集XYZIRT格式的激光点云数据.
  但问题是采集到的数据仍然有不可直接用，速腾格式的点和Velodyne格式的点还是有问题的。这又如何解决呢？笔者在某hub上找到了一位大佬的转化节点，这一节点可将速腾格式的点转化为Velodyne格式的点，具体安装步骤参考笔者另一篇教程链接: 速腾聚创雷达点云格式转换为Velodyne雷达点云格式.
其他雷达笔者没有，无能为力了。

( 1.3GPS 数据格式要求)

  留个小坑吧，笔者还没试融入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

将NLOPTConfig.cmake文件移动到 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接口来替换掉里程计接口。所以这一工具一定程度上并不是精确标定上述两种传感器的，但不精确不代表不可用，能建图就很香。改写这一接口的大佬没找着，没法连接了所以侵删联系。以下是做法：
打开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两分钟即可，最好将数据量控制到2G以内，超出2G之外可能出现未知错误无法标定。两分钟的数据应该尽量包含较大的旋转量和平移量（也就是先直线跑一段再转两圈），这样标出来的误差结果（据说）会收敛到几百以下（已经相当可以了），笔者随便找了数据包标了两三次，还是在17000左右吧，凑合用吧。（标定过程较为漫长，耐心等待）。

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_utils，使用Allan方差来标定高斯白噪声和随机游走噪声（即零偏Bias）。有了这一项参数即可准备开始建图了。

3.1下载imu_utils

  具体操作步骤这里参考了大佬文章，可直接跳过去按照大佬教程安装用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定.

3.2准备标定需要的数据包

  这里要录制两个小时的IMU数据，注意！！！录制数据时必须保持IMU的静止状态，录制全程不可晃动只录制IMU的Topic即可，不过别担心内存，两小时下来只要300M多一点。并且这个数据包最好是放到你imu_utils的工作空间下（与src并列的地方）。最好是IMU上电10分钟之后在开始录制，上电瞬间开始录制会有较大误差。录制指令就不细说了，老司机基本功。

3.3修改launch文件

  打开imu_utils下面有一堆launch文件，将A3.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配置参数

  终于到了这里，终于获得了所需的所有的参数配置，打开config文件夹下params.yaml文件，（最好是copy一份源文件保存下来）。注意这里仅仅是使用了激光雷达和IMU，未配置GPS。

4.1 修改订阅话题

  在红框的地方修改雷达和IMU订阅话题

4.2修改IMU内参

  在上面IMU Settings的地方就是修改内参的地方，打开第三步标定好的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并使用LIO-SAM算法进行建图的实操步骤，并未进行理论分析，下面将给出一些笔者参考过的文献。

  建图效果读者自评吧，参数还没有优化到最好，只进行了最粗略的标定，尤其是激光雷达与IMU的外参并没有收敛到三位数，但毕竟是融合了激光雷达和惯性导航的紧耦合算法，比起笔者测试过的其他单激光雷达传感器算法效果应该是上了一个台阶的。从看到LIO-SAM的建图效果开始尝试适配自己的数据集一直到出现上述效果大概断断续续经历了俩月吧，3D-SLAM的新手，以上结果只是笔者建图过程中遇到的问题以及解决方法，一定还有其他更优解，有更优秀的解法欢迎留言讨论。

以下是笔者参考过的文章，感谢各位大佬分享：

安装教程参考：

LIO-SAM：配置环境、安装测试、适配自己采集数据集.

各种知识点参考:

1.激光SLAM运行LIO-SAM时存在的一些问题.

2.LIO-SAM源码阅读分析（1）–配置文件Tips.

3.Lidar与IMU标定代码实战：lidar_align.

4.【学习总结】Lidar与IMU标定.

5.LIO-SAM论文翻译.

6.用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定.

7.IMU误差模型和校准.

8.lio-sam源码解析，重点：imu预积分.

9.激光雷达和IMU联合标定并运行LIOSAM.

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_42141088/article/details/118000544