IMU+激光雷达融合使用LIO-SAM建图学习笔记——详细、长文、多图、全流程
第一章 前言
写在前面：本文适用于任何想要学习并复现、完成LIO-SAM实地建图的朋友，以下内容全部都是我跟着网络上各个教程，最后成功完成建图的过程记录，文章中包括常规步骤以及报错解决办法等，关于原理则很少涉及。另外本人也是刚刚接触这个领域的小白，只会照猫画虎，因此文章中难免存在不足之处，欢迎批评指正，如有侵权联系删除。
1、先决条件
下面是我建图的一些软件、硬件等基本条件，如果部分参数跟我的不一样，则可能会出现本文之外的问题。
| 项目 | 条件 | 备注 |
|---|---|---|
| 系统 |
ubuntu 20.04 Noetic | 版本不同时，涉及版本号的所有代码中须将neotic换成你自己的版本，如(melodic、kinetic) |
| 软件 | ROS 1.16.0 | 好像无所谓 |
| 激光雷达 | Velodyne VLP-16 | 不是这个的话，浙大外参标定时需要改代码；liosam实地建图时点云格式需要转换 |
| IMU | 9轴超核 | 不是这个也无所谓，只是启动的方式不一样 |
| 其他硬件 | 搭载了Kinetic版本的 jackal 小车，已完成激光雷达、IMU的配置 | 没有也无所谓，只是启动雷达和imu的方式不一样 |
2、全文框架
下面是全文框架，可快速得到文章的结构。
目录
第一章 前言
1、先决条件
2、全文框架
3、建图流程
第二章 运行示例数据集
1、安装ROS
2、安装LIO-SAM
2.1、源码、数据集下载
2.2、安装gtsam
2.3、建立工作空间
ERROR：
2.4、编译LIO-SAM
ERROR：
E1、openCV报错
E2、C++版本报错
E3、/usr/bin/ld 报错
2.5、运行LIO-SAM
ERROR：
3、运行示例数据集
ERROR：
E1、无法运行LIO-SAM程序
E2、rosbag play无法播放数据包
E3、RVIZ建图漂移
E4、保存PCD失败
4、参考链接
第三章 IMU内参标定
1、编译imu_utils
1.1、安装依赖
1.2、安装ceres库
a、安装依赖库
b、下载并解压 ceres1.14.0
c、编译
1.3、编译code_utils
ERROR：
E1、backward.hpp报错
E2、CV_LOAD_IMAGE_GRAYSCALE报错
E3、CV_MINMAX报错
E4、CV_LOAD_IMAGE_UNCHANGED报错
E5、C++问题
1.4、编译imu_utils
ERROR：
E1、C++问题
E2、out_t报错
2、IMU采集数据
2.1、启动IMU
2.2、录制数据
3、IMU标定
3.1、修改配置文件
3.2、标定
a、运行标定程序
b、执行标定流程
c、保存标定结果
3.3、ERROR：
4、参考链接
第四章 Lidar-IMU外参标定
1、编译LI_calib
1.1、建立工作空间并下载源码
1.2、安装依赖
1.3、编译
ERROR:
2、录制数据
2.1、启动IMU和激光雷达
2.2、录制数据
3、外参标定
3.1、修改配置文件
3.2、标定
a、运行标定程序
b、执行标定流程
c、保存标定结果
3.3、ERROR
4、参考链接
第五章 LIO-SAM实地建图
1、录制数据
2、修改配置文件
1.1、基本参数
1.2、内参数据
1.3、外参数据
1.4、其他可选参数
3、实地建图
总结
3、建图流程
下面是完成LIO-SAM复现、建图的大体流程。
- 运行示例数据集——使用官方示例数据集初步跑通LIO-SAM算法
- IMU内参标定——获得IMU内参标定参数
- Lidar-IMU外参标定——获得激光雷达与IMU的位置转换关系参数
- 实地建图——使用IMU+激光雷达建立实地三维点云模型
第二章 运行示例数据集
这一章，我们将搭建LIO-SAM环境、安装LIO-SAM并使用官方示例数据集初步跑通LIO-SAM算法，为完成其复现、建图迈出第一步。
1、安装ROS
ROS的安装在CSDN上浩如烟海，本文重点也不在这里，因此只给出我当初主要参考的ROS安装文章：
【ROS】在 Ubuntu 20.04 安装 ROS 的详细教程_ubuntu20.04安装ros_AlphaCatOvO的博客-CSDN博客
2、安装LIO-SAM
2.1、源码、数据集下载
这里下载源码是为了 2.4编译，编译有两种方法，自己下载然后放入工作空间编译，或者直接进行 git clone（最好还是吧，搞这个不太不方便了hh），如果2.4编译过程中使用git clone命令网速尚可，便不用在此下载源码；下载示例数据集是为了运行，初步跑通LIO-SAM程序，我选择的是其中的 walking_dataset.bag 。
LIO-SAM的源码链接：
GitHub - TixiaoShan/LIO-SAM: LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
示例数据集链接：
https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq
2.2、安装gtsam
先下载，可以在此网页下载，也可以终端使用以下命令：
wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip下载好了以后，直接在下载目录右键解压到当前文件夹（无需到工作空间），然后进入~/Downloads/gtsam-4.0.0-alpha2文件夹，终端打开使用以下命令安装：
cd ~/Downloads/gtsam-4.0.0-alpha2/
mkdir build
cd build
cmake ..
sudo make
make install2.3、建立工作空间
终端输入：
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ~/catkin_ws/
catkin_make
echo "source ~/catkin_ws/devel/setup.sh" >> ~/.bashrc
#最后一步是为了省去每次运行liosam都要source一下的问题，没有也可ERROR：
下面这段没遇到可以不看，直接到2.4。
这里额外插一句我当时踩的坑，如果你不小心在编译完成以后把工作空间下的build、devel文件夹全永久性删掉（当时我因为后续编译IMU内参标定工具的时候有一个模块没装好，然后听信了chatgpt的谗言(苦笑)，结果心态崩了，卡在这卡了两天），如果再次重新编译出现会以下报错：
CMake Error at CMakeLists.txt:68 (message):
find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.这种情况我遇到以后特别烦，查遍了各种攻略也都无济于事，具体表现：
a.每次catkin_make编译时，都会出现以上报错；
b.每次打开终端，最顶端都会自动出现：bash: /home/***/devel/setup.bash: 没有那个文件或目录，根据其他教程删掉~/.bashrc文件里的export行，虽然解决了终端的bash鬼魂问题，但编译报错仍然存在；
c.删掉并重建、初始化工作空间，在devel文件夹下不会生成setup.bash文件。
对于这种情况，我目前还不知道该怎么解决，但最终的暴力解决办法是：卸载重装ROS。ps：一般来说，你装过ROS再卸载重装，熟练的话整个流程不超过15分钟。重装ROS以后就可以正常编译了。
2.4、编译LIO-SAM
下面是编译的一般流程，终端输入：
cd ~/catkin_ws/src
git clone https://github.com/TixiaoShan/LIO-SAM.git
cd ..
catkin_make
ERROR：
如果你和我是在同等环境、软件情况下进行编译，那么以下的报错在一般都会遇到，所以也可以先不编译，把下面这些文件里对应的字段全部改掉以后再编译，会体验无比畅通的编译过程；也可以出现问题，解决问题以后再 catkin_make 重新编译。
E1、openCV报错
示例如下：
解决办法：
用文本编辑器打开 ~/catkin_ws/src/LIO-SAM/include/ 文件夹下的 utility.h 文件，替换其中的 #include
E2、C++版本报错
示例如下：
解决办法：
找到 ~/catkin_ws/src/LIO-SAM/ 文件夹下的 CMakeList.txt 文件，替换其中的 set(CMAKE_CXX_FLAGS "-std=c++11") 这一行，改为：set(CMAKE_CXX_FLAGS "-std=c++14")，保存退出。
E3、/usr/bin/ld 报错
示例如下：
解决办法：
在刚刚的 CMakeList.txt 文件中加入以下这一行：
find_package(Boost REQUIRED COMPONENTS timer thread serialization chrono)至此，不出意外的话应该可以编译成功了（成功的标志是出现100%且光标退出当前命令），如果出现了以上以外的报错，请移步本章 4、参考链接，看下其他几位大佬给出的解决办法。
2.5、运行LIO-SAM
编译完成以后终端输入以下命令即可运行 LIO-SAM 程序：
roslaunch lio_sam run.launch但如果出现：
RLException: [run.launch] is neither a launch file in package [lio_sam] nor is [lio_sam] a launch file name
The traceback for the exception was written to the log file则进入~/catkin_ws/文件夹，终端打开输入：
source devel/setup.bash再输入以上LIO-SAM的运行命令，即可运行并弹出RVIZ的窗口。（多嘴一句，以后遇到其他软件包编译好了以后运行的时候，也出现这种“neither a launch file in package [****] nor is [****] a launch file name”命令的时候，都可以在工作空间的src同级路径下，运行 source 命令）
ERROR：
第一次运行的时候，一般会出现以下报错：
ERROR: cannot launch node of type [robot_localization/ekf_localization_node]: robot_localization
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/*****/catkin_ws/src
ROS path [2]=/opt/ros/noetic/share
ERROR: cannot launch node of type [robot_localization/navsat_transform_node]: robot_localization
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/*****/catkin_ws/src
ROS path [2]=/opt/ros/noetic/share解决办法：
sudo apt-get install ros-noetic-fake-localizationsudo apt-get install ros-noetic-robot-localization除此之外，我记得第一次运行的时候会有几处黄色警告但能打开RVIZ窗口，有几个模块不能正常启动，其中一个记的比较清楚的是 [lio_sam_imuPreintegration-2] 这个模块启动报错，此时只需要运行以下命令，即可正常启动：
cd /usr/local/lib/
sudo cp libmetis.so /opt/ros/noetic/lib/关于其他几个模块不能正常启动的问题，请自行搜索解决办法。直到每次启动时四个模块都是绿色启动字体，如下图，运行的问题就彻底解决了。
至此，应该可以成功运行了，并弹出空的RVIZ窗口，LIO-SAM也成功完成安装，接下来就可以尝试跑通示例数据集了！
3、运行示例数据集
首先，运行LIO-SAM程序，运行方法查看2.5。
其次，用终端进入 2.1 中下载下来的数据集文件同级路径，输入以下命令即可播放数据集：
rosbag play ***.bag其中，***.bag 是你自己选择的示例数据集的包名，这里，我选择的是walking_dataset.bag 。关于rosbag的常用相关命令，可以查看这个链接，后面会用到。
然后，你就会在RVIZ窗口中看到如下画面：
最后，但也是最重要的，就是导出pcd点云文件。LIO-SAM保存点云的方式是修改配置文件自动保存。具体方法：
a. 用文本编辑器打开 ~/catkin_ws/src/LIO-SAM/config/params.yaml 文件，找到其中的22行和23行，修改 savePCD 的值为 true ，修改 savePCDDirectory 的值为你期望的保存点云文件的路径（注意此处以 / 开头结尾，不带 /home/usr/ 字段的路径）。
a*. 第一次保存时，为了避免尚未完全生成PCD文件ROS就已关闭节点的情况，还需要调高 TIMEOUT_SIGINT 值（一劳永逸），终端运行以下命令：
sudo gedit /opt/ros/noetic/lib/python3/dist-packages/roslaunch/nodeprocess.py
#sudo gedit /opt/ros/melodic/lib/python2.7/dist-packages/roslaunch/nodeprocess.py
#sudo gedit /opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/nodeprocess.py
#请根据自己的 ros distro 选择对应命令，三选一CTRL+F找到 DEFAULT_TIMEOUT_SIGINT ，默认为 6，修改其值为60或100都行。
b. 修改好参数后，即可再次运行示例数据集。在需要终止程序时，按下CTRL+C，程序会在数秒内将5个PCD点云文件保存在之前设置的路径下（注意，多次运行时，会覆盖旧的文件，如有需要请备份之前的数据）。
c. 对于导出的PCD文件，要将其可视化，可在终端输入以下命令实现：
pcl_viewer ***.pcd效果如下图：
ERROR：
E1、无法运行LIO-SAM程序
如果在工作空间下src同级路径下 source 过也无法运行的话，一般来说是编译问题，尝试重新按照流程编译即可。
E2、rosbag play无法播放数据包
请检查命令、路径及文件名是否正确。如果全部正确，考虑是ROS安装的问题，尝试重装ROS。
E3、RVIZ建图漂移
一般来说是设置的播放速率过快，此时终端会出现警告：
[ WARN] [1694864659.157473529]: Large velocity, reset IMU-preintegration!
亲测我的设备以8倍速率播放示例包时会产生明显漂移，建图失败。请启用更低的播放速率。
E4、保存PCD失败
请检查TIMEOUT_SIGINT 值、保存路径是否设置正确。
大功告成！！！至此，我们已经完成了LIO-SAM的安装，并运行了示例数据集，保存了建图点云，初步跑通了LIO-SAM程序！接下来就该用LIO-SAM来实地建图，扫描我们自己的数据了。但在实地建图以前，为了建图的准确性，我们还需要进行内参和外参的标定工作，如果不进行内参外参的标定和适配，直接使用实地收集的数据包进行建图的话，建出来的图很大概率会漂移，从而导致建图失败。接下来两章就来进行内参和外参的标定。
4、参考链接
因作者作本文时间距离第一次安装时间已久，存在遗忘可能，难保大家在参考这篇文章的时候会出现本文以外的问题，因此在下面给出几位大佬的文章，如果出现报错，可以查看他们的文章解决。
LIO-SAM：Ubuntu20.04下的编译与运行_error: cannot launch node of type [robot_localizat_MIKingZCC的博客-CSDN博客
LeGO-LOAM：Ubuntu20.04下的编译与运行_ubuntu20运行lego loam_MIKingZCC的博客-CSDN博客
使用Velodyne16线激光雷达与九轴IMU配置环境与运行LIO_SAM程序学习笔记（一）_站住前面的二哈的博客-CSDN博客
实测 ubuntu 20.04 编译LIO-SAM问题与解决办法_ubuntu20 liosam_月照银海似蛟龙的博客-CSDN博客Ubuntu20.04下的编译与运行LIO-SAM【问题解决】_lio-sam 20.04_学无止境的小龟的博客-CSDN博客
运行LIO-SAM，[lio_sam_imuPreintegration-2] process has died，[lio_sam_mapOptmization-5] process has died_lio-sm四个cpp节点被杀死_库洛洛洛洛洛的博客-CSDN博客
第三章 IMU内参标定
在实地建图之前，还需要做两个标定工作，首先就是IMU内参标定，可以解决其固有的测量误差问题。这一章，我们将使用港科大的imu_utils 工具对IMU进行内参标定，包括工具包的编译、数据采集、标定运行。完成后，将得到四个标定参数，对应修改LIO-SAM配置文件，使得建图更精确适用。
1、编译imu_utils
1.1、安装依赖
sudo apt-get install libdw-dev1.2、安装ceres库
a、安装依赖库
sudo apt-get install liblapack-dev libsuitesparse-dev libgflags-dev
sudo apt-get install libgoogle-glog-dev libgtest-dev
sudo apt-get install libcxsparse3b、下载并解压 ceres1.14.0
wget ceres-solver.org/ceres-solver-1.14.0.tar.gz
tar -zxvf ceres-solver-1.14.0.tar.gzc、编译
cd ceres-solver-1.14.0
sudo mkdir build
cd build
sudo cmake ..
sudo make
sudo make install1.3、编译code_utils
注意：这个标定工具有两个包需要编译，分别是 code_utils 和 imu_utils ，而imu_utils依赖code_utils，所以建议新建工作空间（不建议放在先前的catkin_ws这个空间下），并先编译code_utils，再把 imu_utils包放入src目录下进行编译。
这部分对于不熟悉编译流程的朋友来说可能比较绕，接下来的代码流程是针对这部分朋友，手把手的一步一步跟着做绝对没问题，大佬可忽略一些基础步骤。
cd ~
mkdir -p imu_calib/src
cd ~/imu_calib/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_makeERROR：
E1、backward.hpp报错
示例如下：
解决办法：
打开 /home/****/imu_calib/src/code_utils/src/ 下的 sumpixel_test.cpp 文件，用文本编辑器打开，把第二行的 #include "backward.hpp" 改为 #include "code_utils/backward.hpp" 即可。
E2、CV_LOAD_IMAGE_GRAYSCALE报错
示例如下：
解决办法：
在刚刚那个文件里第二行后插入一行 #include"opencv2/imgcodecs/legacy/constants_c.h" 即可。
E3、CV_MINMAX报错
示例如下：
解决办法：
在刚刚的文件中，CTRL+F 将文中两处 CV_MINMAX 改为 NORM_MINMAX 即可。
E4、CV_LOAD_IMAGE_UNCHANGED报错
示例如下：
解决办法：
用文本编辑器打开 /home/****/imu_calib/src/code_utils/src/ 下的 mat_io_test.cpp 文件，把其中的 CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED 即可。
E5、C++问题
C++问题已是老生常谈了，打开 /home/****/imu_calib/src/code_utils/ 下的 CMakeLists.txt 文件，修改 set(CMAKE_CXX_FLAGS "-std=c++11") 为set(CMAKE_CXX_FLAGS "-std=c++14") 。
至此，code_utils就编译完成了，接下来只需要编译imu_utils，便可进行imu的标定。
1.4、编译imu_utils
编译好code_utils后，现在编译imu_utils。
cd ~/imu_calib/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_makeERROR：
E1、C++问题
打开 /home/****/imu_calib/src/imu_utils/ 下的 CMakeLists.txt 文件，修改 set(CMAKE_CXX_FLAGS "-std=c++11") 为set(CMAKE_CXX_FLAGS "-std=c++14") 。
E2、out_t报错
示例如下：
解决办法：
打开 /home/****/imu_calib/src/imu_utils/src/ 下的 imu_an.cpp 文件，用文本编辑器打开，在一种#include*** 行下添加一行 #include
至此，环境就已经安装好了，接下来的就是录制数据和标定。
2、IMU采集数据
接下来需要采集2小时以上的IMU静止数据。
2.1、启动IMU
启动IMU的方法因人而异，我的是集成在jackal小车上的超核9轴IMU，如果你是其他方式比如通过USB连接你的电脑，启动IMU的指令需要参考你的IMU产品手册或者其他博主的文章，以下是我的方案：
进入小车终端，输入以下指令即可打开IMU：
roslaunch imu_launch imu_msg.launch如果报错 ERROR Unable to open port ，则输入以下指令赋予USB串口权限：
sudo chmod 777 /dev/ttyUSB*再次打开IMU，应该可以看到实时变化的IMU数据。
2.2、录制数据
打开IMU后，应等待10分钟再开始录制（上电10分钟之内误差会较大）。录制时应保持IMU（小车）完全静止不动2小时。录制指令为：
rosbag record -O ***.bag /IMU_topic
#其中，***.bag是你期望保存的文件名；
#/IMU_topic是你自己的IMUtopic名称，可使用rostopic list命令查看
#下面是我的指令：
rosbag record -O imu.bag /IMU_data除了以上代码框的注意事项，使用 jackal 小车的朋友还需要注意，以上指令可以在小车终端(主机)输入，也可以在经过ssh和配置环境变量(MASTER_URI和IP)后的PC终端(从机)输入，建议是在小车终端输入，这样不容易丢失数据，录制完以后小车终端输入以下命令，即可复制数据包到PC：
scp @:
#其中，是小车终端上你的数据包文件路径；
#是你PC的用户名;
#是你PC的IPv4地址，可使用 hostname -I 命令查看，结果中的第一个字段便是IPv4地址；
#是你希望保存的PC上的目标路径。
#以下是我的示例：
scp /home/administrator/imu.bag [email protected]:/home/lyf 至此，已经有了数据，接下来要做的，就是把IMU丢在一边，使用 1 中编译好的工具进行标定。
3、IMU标定
3.1、修改配置文件
打开 ~/imu_calib/src/imu_utils/launch 目录，可以看到下载下来的 imu_utils 包中有几个示例 **.launch文件，在这里可以复制任何一个文件副本到本目录下，并随意命名，但要注意后续命令会用到此文件名。在这里我设置的是 jackal_imu.launch 。用文本编辑器打开以后，根据下面的注意事项修改你的对应项（下面代码框中是我的设置）：
注意，上述代码框中：
- "imu_topic"的值是你在 2 中录制的数据包中的imu话题，可使用 rosbag info ***.bag 命令查看对应的imu话题，并注意以 / 开头，虽然info命令不显示斜杠，但也必须加上斜杠；
- "imu_name"的值是你自己的imu名字，可随便起名；
- "data_save_path"的值是你期望保存的标定结果的具体路径，由于imu_utils/data目录下默认有很多文件，后续标定以后不好判断结果文件的生成，故这里可以选择像我一样更改路径，也可不更改路径，然后删除data文件夹里的所有数据，以方便后续判断标定结果是否生成；另外注意以 / 结尾，代表在此目录下生成；
- "max_time_min"的值是你在 2 中录制的数据包的时长，单位分钟，120代表2h，可使用 rosbag info ***.bag 命令查看数据包时长(duration)，并注意在这里设置的分钟数不能高于数据包的实际时间（比如我的数据包duration是1h59min59s，那在launch文件中就只能设置119）。
3.2、标定
a、运行标定程序
终端输入以下命令即可运行：
cd ~/imu_calib
source devel/setup.bash
roslaunch imu_utils jackal_imu.launch
#上述 jackal_imu.launch 应更改为你在本文 3.1 中设置的launch文件的文件名不出意外的话应该可以看到最后一行显示 wait for imu data 。
b、执行标定流程
在存放数据包的路径下打开终端，输入以下指令，以200倍速播放数据包：
rosbag play -r 200 imu.bag等待一分钟左右，数据包即播放完毕，一般来说播放完毕瞬间会显示计算过程，如下图，wait for imu data 行下面出现了计算过程：
然后程序在数秒内会计算完成，显示如下图：
到现在，IMU内参标定的工作就完成了！
c、保存标定结果
完成标定以后，标定的结果文件在之前3.1修改配置文件中设置的路径下，生成的一堆文件内会有一个 ****_param.yaml 文件，这就是我们要的最终结果文件，建议妥善保存，在第五章最终建图时会用到。
3.3、ERROR：
关于IMU内参标定时候的问题，如果是一直显示 wait for imu data ，播放数据包过后很久都没有显示计算过程，原因一般是配置文件(***.launch)配置错误，请仔细检查3.1中关于各项值的注意事项。
DONE！！！至此，IMU的内参标定工作算是完成了，而在实地建图以前，还需要进行雷达和IMU的外参标定，这一部分将在第四章介绍。
4、参考链接
使用imu_utils工具标定imu的内参_imu_utils标定结果_大聪明墨菲特的博客-CSDN博客
利用 imu_utils 标定 imu_匍匐的狗仔的博客-CSDN博客
Ubuntu20.04编译并运行imu_utils，并且标定IMU_学无止境的小龟的博客-CSDN博客
ROS学习篇之传感器（二）IMU（超核IMU HI266六轴/HI13MON 九轴）_9轴imu_张一根的博客-CSDN博客
ros与STM32通讯报错：Unable to open port_爱玩lol 的研究僧i的博客-CSDN博客
第四章 Lidar-IMU外参标定
上一章提到，实地建图以前，还需要进行雷达和IMU的外参标定(以下简称“外参标定”)。外参标定的目的是获得激光雷达和IMU之间的位置转换关系，其中包括平移关系和旋转关系，分别对应最终输出结果中的平移向量与旋转矩阵。
本文采用浙江大学开发的 lidar_imu_calib 工具（以下简称LI_calib）进行外参标定，原因是其他标定工具在各个方面都有一定局限（如苏黎世联邦理工的 lidar_align 工具作者提到不能用于纯imu与雷达的标定；哈工大的 lidar_imu_calib 只能标定旋转矩阵），而后续建图的实践也证明此方法具有较好的鲁棒性。此外，本工具只支持VLP-16与IMU进行标定，如果是其他型号的激光雷达，需要修改部分代码进行适用，网络上有很多修改的示例，请君自行寻找。
1、编译LI_calib
1.1、建立工作空间并下载源码
mkdir -p ~/catkin_li_calib/src
cd ~/li_calib/src
catkin_init_workspace
git clone https://github.com/APRIL-ZJU/lidar_IMU_calib.git1.2、安装依赖
wstool init
wstool merge lidar_IMU_calib/depend_pack.rosinstall
wstool update
cd lidar_IMU_calib
./build_submodules.sh1.3、编译
cd ~/li_calib
catkin_makeERROR:
目前，笔者就遇到一个问题，就是ndt_omp包的C++版本问题，这个问题也是老生常谈了，ubuntu20.04一般使用C++14版本，只需打开 /home/****/li_calib/src/ndt_omp/ 下的 CMakeLists.txt 文件，在第三行加入 set(CMAKE_CXX_FLAGS "-std=c++14") 即可成功编译。
至此，外参标定的环境就配置好了，接下来就是录制数据并进行标定。
2、录制数据
2.1、启动IMU和激光雷达
由于每个人情况不同，启动这两个机器的方式也不同。我使用的是集成在jackal小车上的IMU和激光雷达，启动IMU的方式见第三章2.1，而激光雷达则是随着小车开机自启，无需专门的启动命令。关于其他一般方式比如USB连接两个设备的情况，请自行搜索配置和启动方法。
2.2、录制数据
录制数据时，需要将两个设备充分固定到一起，并基于充分的运动激励。我的两台设备是集成在小车上的，想要笨重的小车做充分的运动（尤其是z轴的平动和三轴转动）实在困难，因此我选择把这两个设备的框架整体拆卸下来，录制好数据以后再整体装回去（当然供电和连接还是由小车供给，且拆装过程中这两个设备要保持完全相对静止）。
录制时其他的注意事项：
- 需要在平面多的房间里录制，特征点过少或者平面过少可能会导致失败，经测试一般办公室环境就可以；
- xyz三轴方向都需要充分移动、充分转动，但不宜加速度过大的猛烈撞击式运动，具体操作可以看 这个 老哥的视频。
录制的流程与IMU数据录制的流程基本一致，具体可参考第三章2.2，只是录制的指令为：
rosbag record -O ***.bag /lidar_topic /IMU_topic
#其中，***.bag是你期望保存的文件名；
#/lidar_topic是你自己的激光雷达topic名称，必须使用生成有序点云的话题；
#/IMU_topic是你自己的IMUtopic名称，可使用rostopic list命令查看。
#下面是我的指令：
rosbag record -O li.bag /velodyne_packets /IMU_data
其中，两个斜杠后面分别是雷达和IMU的话题名称，VLP-16雷达的话题必须使用 velodyne_packets 而不是 velodyne_points ，否则会生成无序点云而导致后面无法标定；其他雷达也应该使用生成有序点云的话题。
开始录制以后，让两设备做充分运动，整个流程1分钟左右即可。结束以后使用CTRL+C结束录制，留存好录制的数据包。
至此，供标定使用的数据就准备好了，只剩标定了。
3、外参标定
3.1、修改配置文件
打开 ~/li_calib/src/lidar_IMU_calib/launch目录下的 licalib_gui.launch 。用文本编辑器打开以后，根据下面的注意事项修改你的对应项（下面代码框中是我的设置）：
注意，上述代码框中以下几项需要修改：
- "topic_imu"的值是你在 2 中录制的数据包中的imu话题，注意以 / 开头，必须加上斜杠；
- "path_bag"的值是你在 2 中录制的数据包的具体路径，我的是放入工作空间内的首目录；
- "bag_durr"的值是你在 2 中录制的数据包的时长，单位秒，133代表2分钟左右；
- "show_ui"的值改为"true"，以便标定时弹出标定UI；
- "topic_lidar"的值默认为"/velodyne_packets"，不能修改，若修改必须确认自己改的值为数据包中生成有序点云的topic（"/velodyne_points"为无序点云）。
3.2、标定
a、运行标定程序
执行以下命令可运行标定程序：
cd ~/li_calib
source devel/setup.bash
roslaunch li_calib licalib_gui.launch执行程序后，可以发现，数据包设置正确的话，可以读取到数据包，但刚读取完就报红、中断：
这是因为下载下来的源码中在ubuntu20.04运行有些问题，作者有些变量设置了布尔类型但却没有返回值，因此需要修改源码：
- 打开 ~/li_calib/src/lidar_IMU_calib/include/utils/ 下的 dataset_reader.h 文件，修改其中的第101行 bool read(const std::string path,**** ，把其中的 bool 改为 void ；
- 打开 ~/li_calib/src/lidar_IMU_calib/thirdparty/Kontiki/include/kontiki/sensors/ 下的 constant_bias_imu.h 文件，修改其中的第87行 bool LockGyroscopeBias(bool lock) 和第95行 bool LockAccelerometerBias(bool lock) ，把其中的 bool 改为 void 。
修改好之后重新使用 catkin_make 编译一次，然后运行，即可看到以下画面：
b、执行标定流程
在弹出的UI窗口中，依次点击以下按钮：
①初始化(Initialization)
点击一次即可，耐心等待终端出现以下字段即初始化成功：
Ceres Solver Report: Iterations: 31, Initial cost: 4.028359e+06, Final cost: 7.998847e+02, Termination: NO_CONVERGENCE
[Initialization] Done. Euler_ItoL initial degree: 178.919 179.266 -82.6326②数据关联(Data Association)
点击一次即可，耐心等待终端出现以下字段即数据关联成功：
[Association] start ....
Plane type : 2 3 35; Plane number: 40
Surfel point number: 22868
[Association] 21146.2 ms③初始化优化（Batch Optimization）
点击一次即可，耐心等待终端出现以下字段即初始化优化成功：
================ Iteration 0 ==================
Ceres Solver Report: Iterations: 31, Initial cost: 2.108107e+06, Final cost: 1.865830e+04, Termination: NO_CONVERGENCE
============== After optimization ================
[Gyro] Error size, average: 13186; 0.00404703 0.00418349 0.0116175
[Accel] Error size, average: 13186; 0.0288359 0.0256794 0.0211061
[LiDAR] Error size, average: 22868; 0.0492501
P_LinI : -0.0184348 0.00617126 0.0859926
euler_LtoI : 179.582 179.409 80.1761
P_IinL : 0.00216661 0.0199835 -0.0858406
euler_ItoL : 0.510795 0.512208 99.8237
time offset : 0
gravity : 0.0586642 -0.111867 9.78919
acce bias : 0.0143021 -0.00411509 0.0325699
gyro bias : 0.000672399 0.001224 -0.000264742
[BatchOptimization] 29117.3 ms④迭代优化（Refinement）
多次点击，终端出现 [Refinement] 24056.2 ms 字段以后就点击一次，直至每次出来的数据不再变化（具体表现是后续每次迭代，终端弹出的字段都完全一样），即视为标定成功，然后手动退出程序。
PS：根据我的尝试，这个过程比较漫长，因为每次执行程序最后标定的结果都不一样（但相对误差不会太大），运气好时四五轮就能标定成功（数据不再变化），运气差时迭代20轮都不一定能成功，需要有耐心。
c、保存标定结果
标定完成以后，标定的结果文件在之前3.1修改配置文件中"path_bag"设置的路径同目录下，有一个 calib_result.csv 文件，这就是我们要的最终结果文件，建议妥善保存，在第五章最终建图时会用到。同时为了数据尽可能准确，建议重复标定流程三次，留存三次的csv文件，后面取均值使用。
3.3、ERROR
关于外参标定的问题，如果是无法弹出UI窗口，报错中断，原因一般是配置文件(licalib_gui.launch)配置错误，请仔细检查3.1中关于各项值的注意事项。
然后就是一个典型错误，虽然我已经提到多次，但如果你运行程序时看到如下报错：
Can't use 2D indexing with an unorganized point cloud
那就说明你使用的点云是无序点云，激光雷达的话题一定要使用 /velodyne_packets 而不是 /velodyne_points ，当然录制数据的时候也应该指定这个正确的话题。
其他的就是在多次点击 Refinement 的时候，要有耐心，等待数据完全不变化以后再 CTRL+C 结束，保存结果文件。
终于！！！我们在这章完成了激光雷达和IMU的外参标定工作，现在所有准备工作已经全部完成了，离最终实地建图只剩一步之遥。
4、参考链接
浙大开源lidar_imu_calib源码安装过程_lidar-imu-calib_Temperat的博客-CSDN博客
https://www.cnblogs.com/chenlinchong/p/14048969.html
ubuntu20.04使用浙大开源包lidar_IMU_calib_FivPluSeven的博客-CSDN博客
Ubuntu 20.04 issues · Issue #48 · APRIL-ZJU/lidar_IMU_calib · GitHub
第五章 LIO-SAM实地建图
在之前的两章中，我们完成了IMU的内参标定和激光雷达的外参标定工作，现在终于来到了实地建图这一部分。这一章要比较简单，稍微麻烦的就是将两个标定的结果适用到算法中来，其他的只要录取数据包然后用LIO-SAM软件包建图就可以了。
1、录制数据
这里指的是在实地场景下启动IMU和激光雷达进行采集数据，最后生成bag包，以便建图时播放使用。
录制数据的方式与前两章录制的方式无异，需要先启动IMU和激光雷达，然后终端运行以下指令（更多细节参考第三/四章2.2节）：
rosbag record -O ***.bag /lidar_topic /IMU_topic
#其中，***.bag是你期望保存的文件名；
#/lidar_topic是你自己的激光雷达topic名称，对于VLP-16来说，/velodyne_points和/velodyne_packets都行；
#/IMU_topic是你自己的IMUtopic名称，可使用rostopic list命令查看。
#下面是我的指令：
rosbag record -O out.bag /velodyne_points /IMU_data运行录制指令以后，操纵小车 / 手持行走到自己想建图的任何地方（建议行走路线构成一个回环，充分利用LIO-SAM的回环检测和矫正），走完以后CTRL+C退出录制，保存好***.bag 文件，待第三节播放建图用。
2、修改配置文件
前两章提到，两次标定（内参标定和外参标定）最后得到的结果文件会在实地建图中使用，现在我们就要使用这些结果文件，以适用到LIO-SAM的程序，而适用的方式就是修改LIO-SAM的配置文件。
LIO-SAM的配置文件路径是：~/catkin_ws/src/LIO-SAM/config/params.yaml 。修改配置文件分四部分，分别是基本参数、内参数据、外参数据和其他可选参数。
1.1、基本参数
要修改的基本参数如下图：
图中，PointCloudTopic 是你的数据包的激光雷达点云话题，注意和之前不一样，这次不能带有斜杠 / ；imuTopic 是你的数据包的IMU话题，也不能带有斜杠 / ；savePCD 和 savePCDDirectory 在第二章第3节提到过，如果当时改了可以不用再修改。
1.2、内参数据
在本文第三章的3.2c中我们提到，IMU内参标定的最终结果是 ****_param.yaml 文件，找到并打开如下：
我们只要其中的4个值，分别是 gyr_n（平均轴旋转高斯白噪声）、gyr_w（平均轴旋转 bias 随机游走）、acc_n（平均轴加速度高斯白噪声）、acc_w（平均轴加速度 bias 随机游走）。
将这四个值分别粘贴到 params.yaml 中对应的位置，如下图：
1.3、外参数据
在本文第四章的3.2c中我们提到，LI外参标定的最终结果是 calib_result.csv 文件，在这里我选择三次结果取平均，将三次结果合并到一个csv文件中并求取平均，如下图：
我们只要上图中的第2列到第8列数据，在上图中，每一列的数据分别是：
- P_IinL.x、P_IinL.y、P_IinL.z，以激光雷达为参考坐标系时，IMU的三轴坐标，组成了雷达到IMU的平移向量；
- q_ItoL.x、q_ItoL.y、q_ItoL.z、q_ItoL.w，以激光雷达为参考坐标系时，IMU的三轴旋转四元数，可后续转化为旋转矩阵。
之前提到过，外参标定的目的是获得激光雷达和IMU之间的位置转换关系，其中包括平移关系和旋转关系，分别对应最终输出结果中的平移向量与旋转矩阵。对于平移向量，可直接粘贴到配置文件中；但对于旋转矩阵，由于外参最终结果是四元数，因此需要将四元数转化为旋转矩阵。
转化的理论可以参考这个博客，在这里，我们使用一个python脚本实现从四元数转化为旋转矩阵：
#!/usr/bin/python3
import numpy as np
# 输入四元数的值，xyzw分别代表q_ItoL.x，q_ItoL.y，q_ItoL.z，q_ItoL.w
x = 0.000328063
y = 0.000575378
z = 0.765159667
w = 0.643839667
# 计算旋转矩阵
R_IMU_to_LiDAR = np.array([
[1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
[2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x],
[2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
])
# 打印旋转矩阵
print("旋转矩阵 R_IMU_to_LiDAR:")
print(R_IMU_to_LiDAR)
终端输入以下代码运行脚本：
python3 ****.py打印输出下图结果：
旋转矩阵
然后，根据下图修改对应的 params.yaml 配置文件：
配置文件中，extrinsicTrans 代表以雷达为参考系IMU的平移向量，在这里应该填入 calib_result.csv 中的 P_IinL.x、P_IinL.y、P_IinL.z 三个值；extrinsicRot 和 extrinsicRPY 代表以雷达为参考系IMU的旋转矩阵，在这两处填入以上终端输出的旋转矩阵（两者一般来说相同）。
1.4、其他可选参数
这里的参数是可选的，如果修改，会使建图效果更好。
一个是IMU的重力加速度G，在IMU内参修改的配置文件中，处于四个内参下面一行的位置，这个值可以根据外参标定结果文件 calib_result.csv 中的第12列，将其替换即可；
另一个是体素滤波参数，根据室外室内有不同的参数值，我的是一楼走廊，因此采用室内值：
这个体素滤波参数应根据实际情况选用，经本人亲测，正确修改后有明显的改进。
3、实地建图
数据包录制好、配置文件修改好以后，就可以直接进行建图了。
建图的流程在第二章已经提到过，首先终端运行：
roslaunch lio_sam run.launch弹出RVIZ窗口以后，另开终端运行 rosbag play ****.bag ，播放之前录制好的数据包，即可开始建图。我是在办公楼一楼走廊里录的6分钟左右的数据包，以下是我开始建图和最终建图的效果视频：
LIO-SAM实地建图-开始建图
LIO-SAM实地建图-最终效果
然后是几张效果图：
可以看到，建图效果很好，没有漂移、分散、重叠的现象，且我在录制数据的过程中，使用的jackal小车存在一定的“猛冲”、“急停”、“急转弯”的现象，在这种情况下仍然能够完美建图，足见此算法的稳健；还有就是我在使用原配置文件（未标定）进行建图的时候漂移十分厉害，帧与帧之间的画面甚至不能同框，足见标定的重要。
CONGRATULATIONS！！！到现在，我们终于完成了IMU和激光雷达融合进行LIO-SAM实地建图，并取得了不错的成果！本文也就到此结束！
总结
在本文中，我们使用IMU和激光雷达两个设备，配合LIO-SAM算法完成了实地建图。首先，我们配置好了LIO-SAM运行的环境，安装了LIO-SAM，并通过运行示例数据集初步跑通LIO-SAM算法；然后，我们分别对IMU的内参和雷达-IMU的外参进行了标定，为实地建图做好了参数辨识；最后，我们借助LIO-SAM成功完成了实地建图，并取得了不错的建图效果。
长文不易，非常感谢大家能够看到这里，作者自知水平有限，此文章中难免存在不足之处，还望大家多多批评指正。
THE END