LIO-SAM从0到1运行自己的数据集
前言
笔者在学习LIO_SAM时踩了不少坑，在此记录从开始到最后整个踩坑过程。文中参考了很多大佬的文章，我只是个搬运工。可以直接跳到第二部分从0到1实现 有疑问可以随时联系我，欢迎交流。
一.LIO-SAM简单介绍 （这一部分可以不看哦，干货在第二部分）
⼀种激光惯导紧耦合的SLAM框架，可在室内和室外实现效果不错的建图。
(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里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。
- 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、发布当前帧原始点云配准之后的点云；
特点：
- 一共使用了三个传感器：imu,激光雷达，GPS（可选也可不选，在此我们主要测试VLP16+IMU的情况）；
- Odometry (IMU frequency) 要收到雷达里程计（lidar odometry）信息后才发出，这样前端频率更高；
- IMU odometry提供初始估计值并做了预积分处理，同时IMU原始数据对雷达进行运动补偿（两个传感器进行数据融合，对于周围环境信息进行更好的估测）；
- 后端因子图优化包括四个因子，IMU预积分结合雷达里程计的帧间约束因子在预积分节点维护，其他三个在后端节点维护，分别是GPS因子、雷达里程计因子和回环检测因子。
- 紧耦合：IMU的零偏可以被估计，利用雷达里程计的帧间约束进行反馈，使IMU解算更加准确，提供更好的初值。
补充说明：
-
IMU通常由三个单轴的加速度计和三个单轴的陀螺仪组成，加速度计检测载体在坐标系统中独立三轴的加速度信号，而陀螺仪检测载体相对于坐标系的角速度信号，对这些信号进行处理之后，便可解算出载体的姿态。IMU的更新频率较高，一般可达几百至1KHz。使用三个加速度值，通过两次积分可获得位移，以此实现位置定位，有角速度值积分可以获取姿态信息，结合在一起可获得物体的实际状态。需要注意的是IMU提供的是相对的原始定位信息，它的作用是测量相对于起点所运动的路线，所以它并不能提供你所在的具体位置的信息。
（9轴IMU有三个单轴加速度计，三个单轴陀螺仪和三个单轴磁力计）
具体的理论学习，源码解读，大家自行去网上搜索（这一部分公开的资料很多），笔者在此只着重介绍如何利用LIO_SAM运行自己的数据集。
二.从0到1实现（可以直接看这里）
1. 效果图
先放张效果图，大概能看出个运行出的效果，还是不错的，笔者当时录包的时候录了所有话题的消息，占用了大量内存，短短的40s视频占了4个G。这是没有必要的哈，我们使用激光雷达+IMU ，真正操作过程中我们只需要录雷达和imu话题（/velodyne_points和/imu/data）就可以了。
2. 硬件介绍
笔者用的是Velodyne16线激光雷达（简称VLP16）+9轴imu（N100）。
网上很多博主介绍跑LIO_SAM要用9轴IMU，6轴IMU也是可以运行的哈，听高博介绍LIO_SAM也是用的6轴。如果需要融合GPS或者资金允许可以采用9轴IMU。为了防止有争议，最好还是用9轴。
3. 前期工作
1）雷达数据格式
LIO-SAM算法对激光雷达的数据格式有着较为严格的要求，以往的单激光雷达建图的算法没注意到这一点，一般要求的是XYZI(x, y, z, intensity ) 格式即可，但是LIO-SAM要求的是 XYZIRT（x, y, z, intensity, ring, timestamp） 格式，即算法内使用了激光雷达的通道数ring参数和时间戳timestep参数，启动算法时会检查是否具有这两个参数。那么如何获得这两个参数呢？ 如果你同笔者使用的激光雷达一样，你可以将激光雷达的驱动更新到RSLidar-SDK版本即可采集，参考大佬另一篇教程安装速腾最新的驱动：(44条消息) Ubuntu18.04 安装速腾聚创最新驱动RSLidar_SDK采集XYZIRT格式的激光点云数据 --SLAM不学无术小问题_摆渡人的博客-CSDN博客
但问题是采集到的数据仍然有不可直接用，速腾格式的点和Velodyne格式的点还是有问题的。这又如何解决呢？大佬在某hub上找到了一位大佬的转化节点，这一节点可将速腾格式的点转化为Velodyne格式的点，具体安装步骤参考笔者另一篇教程链接: (44条消息) Ubuntu18 安装ROS节点解决----速腾聚创雷达点云格式转换为Velodyne雷达点云格式 --SLAM不学无术小问题_摆渡人的博客-CSDN博客_速腾雷达转velodyne（此处搬运大佬的文章，侵删）
如果你使用的激光雷达和笔者是同一款，则不需要考虑这个问题。VLP16的格式就是 XYZIRT（x, y, z, intensity, ring, timestamp） 格式，即算法内使用了激光雷达的通道数ring参数和时间戳timestep参数。
2）坐标系的一致性
（此处搬运大佬的文章，侵删）(44条消息) LIO-SAM运行自己数据包遇到的问题解决–SLAM不学无数术小问题_摆渡人的博客-CSDN博客简单说就是对 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
4. imu的频率设置 （重要）
imu的频率 最好大于100HZ，笔者在用小于100HZ时导致了在跑数据包时，在转弯的时候，地图跟着车转动（准确来说跟着imu转动），一开始没有注意到imu频率问题，一直以为是外参设置的不够准确。其实看过原作者论文的话应该会发现，作者直接把外参设置为单位矩阵，如下图蓝色圈出的部分:平移矩阵和旋转矩阵
但是我们可以从下图官方配置的图看出，vlp16和imu的平移不可能是0，可见跑lio_sam不需要特别精准的外参，如果不相信的话，可以用标定工具标出的结果对比一下看看是不是接近单位矩阵。
笔者在一开始跑的时候，一直对外参和imu的内参的设置要精准坚信不疑，当我发现即使我的外参很精准，跑图还是很飘，就意识到事情的不对了，在此特别感谢两位大佬的提点。
5. imu和vlp16硬件位置的放置 (重要)
-
尽量将imu和雷达的位置放置的近一点，这样平移矩阵设置为0也没问题。
-
imu和雷达的坐标系最好完全一致，如果不能，则尽量保证坐标系相差90°或者180°；
在此插一句题外话，笔者在安装时，一开始没有搞清楚imu的坐标系，整了个乌龙，后来询问商家发现imu的物理坐标系在ROS中输出时已经被调整，而我还按照imu物理坐标系设置，跑出来的图飘的离谱。所以一定要弄清楚获取到的imu数据和雷达坐标系是什么方向的。在此放一张笔者安装图：
笔者的imu和lidar坐标系是安装的完全一致的（Z轴竖直向上），而且靠的也很近，所以笔者后来就直接将平移矩阵设置为0，旋转矩阵设置为单位矩阵（这样是最省事的）。外参设置的也就是imu和lidar的转换关系，如果从源头就解决这个问题，那就会方便很多。
在lio-sam参数文件中，在imu内参下面的有一个参数是imu的重力加速度，正负表示方向：
如果imu和vlp16的Z轴同时向上则设置为正，反之为负。
还需要注意一下波特率和imu频率匹配，115200对应的频率应该是不能超过50HZ，961200可以设置100HZ往上。
6. 激光雷达与IMU外参标定
可以直接跳到链接：(44条消息) LIO-SAM运行自己数据包遇到的问题解决–SLAM不学无数术小问题_摆渡人的博客-CSDN博客_lio_sam这个博主写的很详细。
网上有很多开源的标定方法，笔者使用lidar_align标定成功。
按上述来说不需要特别精准的外参，如果允许程序时发现 警告large bias…large velocity…，那就是外参设置错了，也就是转换关系错了，可以使用这个标定一下，看一下转换关系
（1）安装lidar_align联合标定激光雷达和IMU外参程序
- 下载lidar_align
cd 自己的工作空间/src git clone https://github.com/ethz-asl/lidar_align.git catkin_make
建议下载修正过后的代码：lidar_align_wwtx
新建自己的ROS工作空间，将该源码放入，catkin_make即可。
-
首次编译会因为缺少NLOPT库报错
编译时出现Could not find NLOPTConfig.cmake
解决方法：
sudo apt-get install libnlopt-dev
找到并将NLOPTConfig.cmake文件移动到 lidar_align/src/下（此处可以直接编译），如果还是报上面错误的话则在CMakeLists.txt里加上这样一句话：
list(APPEND CMAKE_FIND_ROOT_PATH ${PROJECT_SOURCE_DIR}) set (CMAKE_PREFIX_PATH "/usr/local/lib/cmake/nlopt")如图：因为身在外地，无法演示，就放一张大佬的图吧 （侵删）(44条消息) 激光雷达和IMU联合标定并运行LIOSAM_浪客_剑心的博客-CSDN博客_liosam
然后进行第二次编译
-
二次编译还是会报错，解决冲突问题（按照顺序，运行下面的命令即可）
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）改写IMU接口
原文链接：https://blog.csdn.net/weixin_42141088/article/details/118000544 （侵删）
这一工具原本不是用来标定激光雷达和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
-
-
- 发布
- 订阅
- 功能简介
- 发布
- 订阅
- 发布
- 订阅
- 功能简介
- 发布
- 订阅
- 功能简介
- 发布
- 订阅
