在使用LIO-SAM进行Lidar+IMU建图之前，需要做两个标定工作，IMU内参标定和Lidar-IMU外参标定。IMU内参标定，可以解决其固有的测量误差问题；Lidar和IMU的外参标定(以下简称“外参标定”)。外参标定的目的是获得激光雷达和IMU之间的位置转换关系，其中包括平移关系和旋转关系，分别对应最终输出结果中的平移向量与旋转矩阵。
标定环境如下：
- OS：Ubuntu20.04
- ROS：1 Noetic（选ROS1的原因是大多数标定工具都没有ROS2版本）
- IMU：H30
- Lidar：Hesai Pandar40P
一、IMU内参标定
IMU的内参标定主要分为确定性参数标定和不确定性参数标定：
- 确定性参数标定：包括尺度、坐标系非正交（安装误差）、传感器偏置, 代表性工具imu_tk。
- 不确定性参数标定：包括量化噪声（Q）、角度随机游走（N）、零偏稳定性（B）、角速率随机游走（K）、速率斜坡（R），代表性工具imu_utils。
IMU自身的重置校准已经通过厂商工具完成，这里主要还需要采用imu_utils工具对IMU进行内参标定。imu_utils的核心旨在通过Allan方差分析来评估IMU（加速度计和陀螺仪）的随机误差特性。它不仅提供了C++实现的Allan方差工具，还支持从静态测试中收集的数据进行细致的噪声分析。用户能够通过这个工具包对不同型号的IMU（如ADIS16448、3DM-GX4等）在静置状态下的数据进行长达两小时的采集，进而分析“白噪声”、“偏置不稳定性”等关键参数，这些对于导航系统和自动驾驶应用至关重要。通过使用imu_utils，研究者和工程师能够更有效地校准和选择适合他们应用的IMU，确保传感器数据的可靠性和精确度。无论是无人机、自动驾驶车辆还是机器人领域，imu_utils都是一个不可或缺的工具箱。
imu方差收敛示意图：
- 左列：分别是加速度计和陀螺的allen方差（可以粗略的判断出我们需要静置imu的时间，即初始化时间）
- 右列：根据标定获得的imu内参，仿真产生的allan方差图
1. 编译imu_utils工具
# 1. ceres-solver
sudo apt-get install libdw-dev libceres-dev
# 2. download code
mkdir -p ~/catkin_calib/src
cd ~/catkin_calib/src
git clone git@github.com:yanjingang/code_utils.git
cd ..
# 3. edit for build bug fix
vim src/code_utils/src/sumpixel_test.cpp
row2: #include "backward.hpp" 改为 #include "code_utils/backward.hpp"
row 84/107: CV_LOAD_IMAGE_GRAYSCALE 改为 cv::IMREAD_GRAYSCALE
row 94/117: CV_MINMAX 改为 CV_MMX
vim src/code_utils/src/mat_io_test.cpp
row 33: CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED
# 4. build
catkin_make -DCATKIN_WHITELIST_PACKAGES=code_utils
catkin_make -DCATKIN_WHITELIST_PACKAGES=imu_tuils2. 采集IMU数据
打开IMU后，等待10分钟再开始录制（上电10分钟之内误差会较大），录制时应保持IMU完全静止不动，时长建议至少两小时左右，否则标定出来的参数不够准确。
# 启动IMU
roslaunch yesense_imu yesense_ahrs.launch
# 录制bag
rosbag record -O h30.bag /sensor/imu/data
# 查看包信息
rosbag info h30.bag
path: h30.bag
version: 2.0
duration: 9hr 41:47s (34907s)
start: Nov 04 2024 23:48:21.41 (1730735301.41)
end: Nov 05 2024 09:30:08.77 (1730770208.77)
size: 2.5 GB
messages: 6981520
compression: none [3249/3249 chunks]
types: sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
topics: /sensor/imu/data 6981520 msgs : sensor_msgs/Imu3. 内参标定
3.1 配置IMU标定参数
# conf
vim src/imu_utils/launch/yesense_h30.launch
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/sensor/imu/data"/>
<param name="imu_name" type="string" value= "yesense_h30"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/yesense_h30/"/>
<param name="max_time_min" type="int" value= "200"/>
<param name="max_cluster" type="int" value= "100"/>
</node>
# build
catkin_make -DCATKIN_WHITELIST_PACKAGES=imu_utils- imu_topic：录制的数据包中的imu话题，可使用 rosbag info ***.bag 命令查看对应的imu话题，并注意以 / 开头，虽然info命令不显示斜杠，但也必须加上斜杠。
- imu_name：你自己的imu名字，可随便起名。
data_save_path：你期望保存的标定结果的具体路径，由于imu_utils/data目录下默认有很多文件，后续标定以后不好判断结果文件的生成，更改路径以方便后续判断标定结果是否生成；另外注意以 / 结尾，代表在此目录下生成。 - max_time_min：录制的数据包的时长，单位分钟，120代表2h，可使用 rosbag info ***.bag 命令查看数据包时长(duration)，并注意在这里设置的分钟数不能高于数据包的实际时间（比如我的数据包duration是1h59min59s，那在launch文件中就只能设置119。
3.2 IMU内参标定
cd ~/catkin_calib
source devel/setup.bash
# 启动标定程序
roslaunch imu_utils yesense_h30.launch
# 200倍速播包（直接启动imu的输出等2-6小时，现采现标也可以，不过建议还是录下来bag，方便重复标）
rosbag play -r 200 h30.bag2小时的包需要等待一分钟左右，数据包即播放完毕，一般来说播放完毕瞬间会显示计算过程：
程序在数秒内会计算完成，并显示finish并自动退出。
查看设定的data_save_path目录下会生成yesense_h30_imu_param.yaml文件，我们只要其中的4个值，分别是 gyr_n（平均轴旋转高斯白噪声）、gyr_w（平均轴旋转 bias 随机游走）、acc_n（平均轴加速度高斯白噪声）、acc_w（平均轴加速度 bias 随机游走）。
将yesense_h30_imu_param.yaml中的这四个内参值放到LIO-SAM/config/params.yaml对应位置：
$ cat ~/catkin_calib/src/imu_utils/data/yesense_h30/yesense_h30_imu_param.yaml
%YAML:1.0
---
type: IMU
name: yesense_h30
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 1.2050330151032497e-03
gyr_w: 1.4177677047615486e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 7.5656614363139437e-03
acc_w: 2.9325075138204079e-043.3 配置LIO-SAM的IMU内参
对应LIO-SAM的IMU内参配置：
vim ~/ros2_ws/src/LIO-SAM/config/params.yaml
# IMU Settings (IMU内参)
imuAccNoise: 7.5656614363139437e-03 # acc_n
imuGyrNoise: 1.2050330151032497e-03 # gyr_n
imuAccBiasN: 2.9325075138204079e-04 # acc_w
imuGyrBiasN: 1.4177677047615486e-05 $ gyr_w
二、IMU与Lidar外参标定
外参标定的目的是获得激光雷达和IMU之间的位置转换关系，其中包括平移关系和旋转关系，分别对应最终输出结果中的平移向量与旋转矩阵。
这里采用浙江大学开发的 lidar_imu_calib 工具（简称li_calib）进行外参标定，它是一个用于校准 6DoF 刚性变换和 3D LiDAR 与 IMU 之间的时间偏移的工具包，基于连续时间批量优化，建图的实践证明此方法具有较好的鲁棒性。这个工具只支持VLP-16与IMU进行标定，我们使用的Pandar40P需要进行适配才能使用。
1. 源码编译
最好科学上网执行以下操作，能节约很多时间：
# depend
sudo apt install -y python3-wstool ros-noetic-pcl-ros ros-noetic-velodyne-msgs ros-noetic-velodyne-pointcloud
# download
cd ~/catkin_calib/src
# slam code
git clone git@github.com:yanjingang/ndt_omp.git # 这个是使用c++14编译的版本
git clone git@github.com:yanjingang/li_calib.git
# sensor code
git clone https://github.com/HesaiTechnology/HesaiLidar_General_SDK.git
git clone --recursive https://github.com/HesaiTechnology/HesaiLidar_General_ROS.git
git clone git@github.com:yanjingang/hesai_to_velodyne.git
git clone git@github.com:yanjingang/yesense_ros1.git
# depend pangolin
cd li_calib
./build_submodules.sh
# build
cd ~/catkin_calib
catkin_make -DCATKIN_WHITELIST_PACKAGES=ndt_omp
catkin_make -DCATKIN_WHITELIST_PACKAGES=li_calib
catkin_make -DCATKIN_WHITELIST_PACKAGES= # 清除包指定，全编译
source ./devel/setup.bash2. 数据采集
录制数据时，需要将Lidar-IMU设备固定到一起，并基于充分的运动激励。
- 需要在平面多的房间里录制，特征点过少或者平面过少可能会导致失败，经测试一般办公室环境就可以。
- XYZ三轴方向都需要充分移动、充分转动，但不宜加速度过大的猛烈撞击式运动，可各轴方向正常转动4-5次， 具体方法可以参考这里 。
- 整体时间1分钟左右即可。
开始录制：
cd ~/catkin_calib
source devel/setup.bash
# 启动Lidar (Frame: Pandar40P, Topic: /hesai/pandar)
roslaunch hesai_lidar hesai_lidar.launch
# 启动IMU (Frame: imu_link, Topic: /sensor/imu/data)
roslaunch yesense_imu yesense_ahrs.launch
# 开启Lidar PTP时钟同步
sudo ptp4l -m -4 -i eno2 -S
# 检查帧率
rostopic hz /sensor/imu/data /hesai/pandar
topic rate min_delta max_delta std_dev window
/sensor/imu/data 200.0 0.003751 0.006297 0.001005 201
/hesai/pandar 10.01 0.09922 0.1012 0.0006164 201
# 检查时间戳是否正常
rostopic echo /hesai/pandar -p
rostopic echo /sensor/imu/data -p
# 启动录包
rosbag record -O pandar40p-h30.bag /hesai/pandar /sensor/imu/data
# 查看包信息
rosbag info pandar40p-h30.bag
path: pandar40p-h30.bag
version: 2.0
duration: 3:19s (199s)
start: Nov 06 2024 23:48:12.51 (1730908092.51)
end: Nov 06 2024 23:51:32.15 (1730908292.15)
size: 5.9 GB
messages: 41925
compression: none [1997/1997 chunks]
types: sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics: /hesai/pandar 1996 msgs : sensor_msgs/PointCloud2
/sensor/imu/data 39929 msgs : sensor_msgs/Imu3. 外参标定
3.1 修改配置
根据录制数据修改Lidar类型、Topic、Bag位置、录制时长等参数：
vim ~/catkin_calib/src/li_calib/launch/licalib_gui.launch
<arg name="lidar_model" default="HESAI_40P" /> <!-- Lidar类型: VLP_16 / HESAI_40P / RS_16 -->
<arg name="topic_imu" default="/sensor/imu/data" /> <!-- Lidar topic -->
<arg name="topic_lidar" default="/hesai/pandar" /> <!-- IMU topic -->
<arg name="path_bag" default="/home/work/catkin_calib/pandar40p-h30.bag" /> <!-- 录制的数据包的具体路径 -->
<arg name="bag_durr" default="120" /> <!-- 录制的数据包的时长，单位秒 -->
<arg name="show_ui" default="true" /> <!-- 打开可视化操作窗口 --> *注：如果是velodyne的雷达，需要录制/velodyne_packets 而不是 /velodyne_points才可以。我这里用的Pardar40P，是在li_calib里单独适配了下格式，适配的时候注意sensor_msgs::PointCloud2和velodyne_msgs::VelodyneScan格式是2个不同的unpack_scan()，根据你的雷达只适配其中一个方法即可。
3.2 启动标定程序
cd ~/catkin_calib
source devel/setup.bash
roslaunch li_calib licalib_gui.launch3.3 Initialization初始化
在弹出界面上，点击Initialization按钮进行初始化，控制台输出如下：
Load dataset lidar_type=2 bag=/home/work/catkin_calib/pandar40p-h30.bag
/hesai/pandar: 1980
/sensor/imu/data: 39601
Ceres Solver Report: Iterations: 29, Initial cost: 6.809535e+06, Final cost: 5.975024e+02, Termination: CONVERGENCE
[Initialization] Done. Euler_ItoL initial degree: 178.901 178.979 -86.11023.4 Data Association数据关联
点击Data Association按钮进行数据关联，控制台输出如下：
[Association] start ....
Plane type :32 12 19; Plane number: 63
Surfel point number: 46353
[Association] 201426 ms
关联成功后ui界面上会显示出来采集数据的地图：
3.5 Batch Optimization迭代优化初始化
点击Batch Optimization按钮进行迭代优化初始化，控制台输出如下：
================ Iteration 0 ==================
Ceres Solver Report: Iterations: 31, Initial cost: 7.883758e+06, Final cost: 1.245670e+05, Termination: NO_CONVERGENCE
============== After optimization ================
[Gyro] Error size, average: 39579; 0.00828549 0.00818175 0.00496492
[Accel] Error size, average: 39579; 0.028903 0.0306898 0.0593846
[LiDAR] Error size, average: 46353; 0.0838659
P_LinI : 0.0329959 -0.0504669 0.0173797
euler_LtoI : 0.765177 0.508529 -93.3331
P_IinL : -0.0482363 -0.0356993 -0.0183443
euler_ItoL : 0.552158 -0.734316 93.3333
time offset : 0
gravity : -0.670243 -0.65516 9.74503
acce bias : -0.0602972 0.137668 -0.0115294
gyro bias : -0.00037345 0.000105357 0.000272736
[BatchOptimization] 59086.6 ms3.6 Refinement迭代优化
点击Refinement按钮进行迭代优化，与上一部的初始化输出类似，但是需要多次点击。终端每出现 [Refinement] 24056.2 ms 信息以后就点击一次，直到每次终端显示的字段都完全一样不再变化（一般5-20轮左右），即视为标定成功，然后手动退出程序。标定结果会保存在bag包相同路径的同名文件夹下calib_result.csv 文件内。
cat ~/catkin_calib/pandar40p-h30/calib_result.csv 3.7 配置LIO-SAM的Lidar -> IMU外参
标定外参要提供给LIO-SAM使用，因为涉及到四元数转化为旋转矩阵以及对应关系等，解释起来有点繁琐，我这里直接写了个自动生成脚本来根据Lidar-IMU标定外参一键生成LIO-SAM的配置参数：
$ python3 tools/gen_liosam_extrinsics.py ~/catkin_calib/pandar40p-h30/calib_result.csv
p_IinL.x p_IinL.y p_IinL.z q_ItoL.x q_ItoL.y q_ItoL q_ItoL.w
0.0169827,-0.0285108,0.097318,-0.00186273,-0.00394953,0.720984,0.692938
0.0169827,-0.0285108,0.097318,-0.00186273,-0.00394953,0.720984,0.692938
Avg: 2 / 21
0.0169827 -0.0285108 0.097318 -0.00186273 -0.00394953 0.720984 0.692938
IMU_to_LiDAR: [[-0.03966705 -0.99917971 -0.00815956]
[ 0.99920914 -0.0396428 -0.00311358]
[ 0.00278756 -0.00827661 0.99996186]]
Extrinsics (lidar -> IMU):
extrinsicTrans: [0.0169827, -0.0285108, 0.097318]
extrinsicRot: [-0.03966705, -0.99917971, -0.00815956,
0.99920914, -0.0396428, -0.00311358,
0.00278756, -0.00827661, 0.99996186]
extrinsicRPY: [-0.03966705, -0.99917971, -0.00815956,
0.99920914, -0.0396428, -0.00311358,
0.00278756, -0.00827661, 0.99996186]
三、总结
至此，在趟了无数个坑之后，我们的Lidar-IMU外参标定工作就做好了，下一次我带大家一起用这个外参SLAM建图看下标定效果是否精准。
参考：
IMU+激光雷达融合使用LIO-SAM建图学习笔记——IMU内参标定/Lidar-IMU外参标定
基于lidar_IMU_calib 的 速腾 RS32 雷达适配
十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）
error: ‘CV_LOAD_IMAGE_UNCHANGED’ was not declared in this scope
禾赛、速腾三维雷达适配lio-sam、fast-lio2建图算法
ubuntu20.04使用浙大开源包适配RS_Helios5515
/velodyne_packets和/velodyne_points的相互转换
关于一些 ROS 驱动程序和传感器校准工具（包括 IMU、Camera、LiDAR 等）的工作区。
error: ‘class pclomp::NormalDistributionsTransform has no member named ‘getTargetCells()’ #56
