ZONGMU-CALIB

有些地方图片或者公式不对，请参考有道云笔记版本：
雷达IMU外参标定总结

1. 问题描述

本工作目标是标定3D激光雷达和IMU的外参关系,以L表示Lidar, M 表示IMU，即求出下面的参数。
$$
{T_{LM}, R_{LM}}
$$
其中$T_{LM}$表示从IMU坐标系到雷达坐标系的平移转换矩阵, $R_{LM}$ 表示从IMU坐标系到雷达坐标系的旋转转换矩阵。

2. 标定过程简介

标定过程主要参考浙江大学2020 IROS 文章: Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation

标定整体框架图如图所示：
lidar imu calibration framework

对于准备好的IMU数据以及3D激光雷达数据，通过如下步骤来获得外参：

首先用手眼标定算法，求出初始旋转矩阵。并用IMU数据分别建立角度B样条与位移B样条曲线。
用初始旋转矩阵对点云做矫正，然后用NDT算法建立点云地图。再对每个网格，计算其中是否是平面，如果是则将网格中的平面称为面元，此地图亦称之为面元地图。
联合优化求解旋转矩阵与平移矩阵。其中，误差项由陀螺仪测量数据与旋转B样条中数据的误差，加速度计测量数据与平移B样条中数据的误差以及点到平面的距离这三者的平方和组成。 $ argmin { (e_a)^2 + (e_b)^2 + (e_c)^2 } $。优化变量为B样条的控制点，雷达IMU相对外参以及IMU内参。
步骤3得到的外参可以用来进一步矫正点云，然后继续建立面元地图，联合优化求解旋转矩阵与平移矩阵。不断迭代优化，直至得到精度较高的优化变量。最终的结果显示为平面数目以及光滑度。可视化的平面越多，越光滑，则结果越精确。【结果对比如图所示】

3. 数据准备

此节说明如何采集有效的标定数据并处理原始数据，使之成为第2节所述的“准备好的数据”。

3.1 数据采集部分

在无人驾驶车辆中，激光雷达一般正放在车顶，尽可能感知大范围的环境。而IMU则一般在车重心的位置（比如后座中心），以尽可能准的反映车辆的角度，加速度的变化情况。
如果IMU也是水平放置的，那么汽车在水平面上移动时，雷达与IMU则只有yaw角的变化。此时激励不足以完成平移矩阵的标定。【但可以完成旋转矩阵的标定】
故采集数据时
- 如果只需要标定旋转矩阵，那么正常放置即可。然后建议有转弯和上坡，以提升数据的有效性。
- 如果需要标定平移矩阵，那么需要将IMU斜置45度。【以水平正放为 z朝上，x朝右，y朝前为例可以先将IMU绕Y轴旋转45度，再绕X或者Z轴旋转45度。】这样放置，当车辆经过转弯以及上坡后，雷达与IMU就有了三个轴上的相对旋转。
备注：IMU肯定是平置在车内的，那么或许可以先通过斜放算出平移矩阵1和旋转矩阵1，然后再平放，算出旋转矩阵2。假设IMU只有绕中心旋转，应该可以推算出平放对应的平移矩阵。

3.2 数据处理部分

首先第一步是对数据做时间对齐操作。此处通过轨迹（角度）对齐来同步时间。（徐鹏）
将六轴IMU数据从 .csv 格式转成 rosbag包。
用互补滤波程序来对 IMU 数据做互补滤波，得到含有滤波后的六轴数据以及积分欧拉角的rosbag包。
将滤波后的rosbag包和雷达的rosbag包合成一个rosbag,这个rosbag包即第2节中“准备好的数据”。
备注：相关的仓库分析，代码分析以及具体操作流程见附录。

4. 外参标定

对于准备好的IMU数据以及3D激光雷达数据，标定有如下几个步骤：

使用手眼标定算法来求解 $R_{LM}$ 。即 $\Delta R_L \times R_{LM} = R_{LM} \times \Delta R_M$。其中 $\Delta R_L$ 和 $\Delta R_M$ 分别表示单位时间内雷达的旋转角和IMU的旋转角。同时根据陀螺仪以及加速计的数据，构建平移矩阵和旋转矩阵的B样条曲线。
利用步骤1求的旋转矩阵 $R_{LM}$ 以及 IMU数据，对点云数据做旋转矫正。然后用NDT来建图。
对建图后的每一个体素，判断其是否是平面。找到所有的平面后，与地图中的点一一对应，建立点与平面的匹配对。
使用优化器来联合优化求解平移矩阵 $T_{LM}$与旋转矩阵 $R_{LM}$。残差函数陀螺仪测量数据与旋转B样条中数据的误差，加速度计测量数据与平移B样条中数据的误差以及点到平面的距离这三者的平方和组成。 $ argmin { (e_a)^2 + (e_b)^2 + (e_c)^2 } $。
步骤4求得的转换矩阵可以返回步骤2。如此多次迭代，可逐渐逼近真实结果。最终的结果显示为平面数目以及光滑度。可视化的平面越多，越光滑，则结果越精确。

备注：相关的仓库分析，代码分析以及具体操作流程见附录。

最后标定的结果如下所示：
下图是手眼标定的结果。图一红线和蓝线分别表示 $\Delta R_L$ 与 $ \Delta R_M$。图2表示 $\Delta R_L \times R_{LM} $ 与 $ R_{LM} \times \Delta R_M $。图三表示 $\Delta R_L$ 与 $ R_{LM} \times \Delta R_M \times R_{LM}^{-1}$
hand eye calibration result
下图是用纵目在地下车库上坡数据集标定出来的结果：
example result

5. 问题总结

测量数据的有效性问题【数据对于标定程序的可观性，数据的质量极其重要】
- 数据可观性的判定过程：
  - 在雷达相机的标定过程中，数据的可观性是通过对非线性最小二乘问题进行优化求解时的公式 $ J^TJ\Delta x= -J^Tr $ , $ H\Delta x= b $, 如果H矩阵存在零空间N，那么便有 $ H\Delta x + HN= b $ ，存在无穷多个解 $ H\Delta x= b $ 使得残差最小，从而解不出最优解。而通过对 H 矩阵做SVD分解，如果其最小的奇异值小于0.001，那么就可以认为此数据中该维度不可观，或者说观测度不够。
  - 在雷达IMU标定中，数据可观性的判断在手眼标定求旋转矩阵过程中，对车辆而言，旋转足够了，应该不存在平移不够。手眼标定公式为:
    $$
    \Delta q_L \times q_{LM} = q_{LM} \times \Delta q_M
    $$
    其对应的非线性最小二乘问题是：
    $$
    argmin{ (\alpha_k ( [q_{L_k}^{L_k+1}]L - [q{M_k}^{M_k+1}]R )) \times q_L^M }
    $$
    其中 $ [q{L_k}^{L_k+1}]L $ 表示雷达k帧和k+1帧旋转角度的左乘四元数，$ [q{M_k}^{M_k+1}]_R $ 表示IMU 的k帧和k+1帧旋转角度的右乘四元数。$ \alpha_k $ 是权重系数。上式可以用H矩阵代替左边公式，得到
    $$
    H \times q_L^M
    $$
    同理，对H矩阵（[n*4][4]）做SVD分解，三维空间旋转的自由度是3，则判断其第三小的奇异值，如果该奇异值过小，则该维度不可观。如果前两个奇异值一样大，后两个都为0，那么此数据只在一个维度可观。
- 什么样的数据才是有效的：
  - 首先分析我处理过的几份数据:
    1. 第一份是作者自己录制的数据。作者捧着小车平台，可以各个角度充分旋转平移，故其判断第三小的奇异值大于0.25 才认为数据可用。其标定的结果也很好，迭代优化几次后就可以得到大量且平滑的平面。
    2. 第二份数据是韩国 KAIST08 数据集，是在一个居民区录制的。这个数据集的特点在于激光雷达是斜45度放在车后面的。我这边选取了一段拐弯的路段来标定。结果第三小的奇异值很小，前两个奇异值是正常的。即数据缺失了一个维度的信息。从而导致初始旋转结果就不对，越优化平面越少。
    3. 第三份数据是纵目地下车库录制的平面上转了一圈的数据，截取了其中两个弯来测试，发现奇异值只有前两个数据非0，而且这两个数据一样。。这意味着雷达IMU都正放且在平面上只转个弯，那么只有一个维度的信息。
    4. 最后一份数据是IMU斜放，车辆既有转弯又有上坡。此数据有三个维度的数据，不过第三小的数据也不大，只有0.15左右。但已经够用了。此时可以得到大量光滑平面，且可以稳定。从结果来看，其平移结果也是比较准的。
  - 可以看到，无论是IMU斜放还是雷达斜放，即使车辆只有yaw角的变化，也可以获得两个维度的数据。这是因为斜着放IMU时，如果雷达只有yaw角转动了90度，那对IMU而言可能同时有 yaw角，roll角，pitch至少两个角度的变化。故有效数据的本质是 INU积分角度和雷达里程计算出来的角度在三个维度上都需要有足够的差异。
  - ==推理：如果雷达正放，IMU斜放，而且雷达的yaw角可以投影到IMU三个轴上。那么车辆只要绕一个8字，应该就可以让数据拥有足够的可观性。==
误差分析
- IMU与雷达的时间戳对齐是通过 yaw 角轨迹对齐法得到的，没有硬件同步。
- IMU的内参标定的程序没见到，使用的是默认参数。
- 互补滤波程序也需要进一步优化，以解决bug和更方便的使用。
B样条，连续时间优化？使用B样条的好处？
- B样条的引入是为了化离散为连续，很契合IMU这种高频传感器，效果好于离散。而且连续函数求导求积分要比离散要准确。此文章的优化变量为【$ q_L^I, p_L^I, x_q, x_p, b_a, b_g $】
- 其中【$ q_L^I, p_L^I$】分别表示IMU坐标系到雷达坐标系的旋转四元数和平移矩阵，通过B样条数据与面元地图来构建误差函数。更具体地，B样条数据通过【 $ q_L^I, p_L^I$】来矫正点云，校正后的点云用来构建NDT点云地图。【$ q_L^I, p_L^I$】越准，那么地图越平整，从而点与面元之间的误差越小。
- 另一方面，【$ x_q, x_p, b_a, b_g $】分别表示旋转四元数和位移B样条中的控制点，以及加速度计和陀螺仪的偏置，通过B样条数据与实际数据来构建误差函数。更具体地，通过优化【$ x_q, x_p, b_a, b_g $】来让B样条曲线尽可能接近真实的角度分布或者平移分布。
- 而作者通过一个联合优化函数同时优化【$ q_L^I, p_L^I, x_q, x_p, b_a, b_g $】，物理意义即使B样条曲线尽可能接近测量数据的同时，用B样条的数据来矫正点云，使得到的点云地图尽可能平整。
结果可视化验证的重要性
- 对于松耦合多步骤程序，每一步的结果都需要做验证，以在出啥问题时快速锁定错误范围。可以通过python程序进行快速结果验证。最好是可视化验证，直观明了。数据计算可能会有疏漏。

附录1：使用的仓库及分析

自己使用的库：

从雷达数据包以及imu csv文件生成rosbag，imu_csv_to_rosbag:
- 读取csv文件，将imu信息写入rosbag中。此IMU信息只有六轴数据。
- 后面再读取雷达数据并写入到新rosbag中
- ==注意雷达与IMU的时间戳要对齐==。【轨迹对齐法，具体咨询徐鹏】
对 imu 做互补滤波, zm_imu_filter_process
- 输入IMU六轴数据，计算互补滤波
- 将校正后的角速度，加速度以及姿态四元数(互补滤波结果)发布到sensor_msgs::Imu.，写入新rosbag 【互补滤波出来的结果需要乘两次 180/pi 才等于欧拉角】
- 读取雷达数据，也写入新rosbag中
标定雷达IMU的旋转矩阵,lidar_imu_hand_eye_calib
- 就是 LIO-SAM推荐标定代码(只标定R) 代码的适配版。
- 有两个python画图程序，一个是看 IMU 与雷达的四元姿态轨迹，一个看手眼标定的结果是否一致【重要】。
- 修改了求解最优化过程。用ceres库代替原来的矩阵分解，结果有些许优化
- 此代码只使用了IMU的四元姿态数据，没有用那六轴数据。【所以结果的精度很依赖互补滤波输出。】
- 数据正常的情况下，求出来的 R 还是挺准的。用画图程序，根据手眼标定公式来判断R求的准不准
标定雷达IMU的转换矩阵,zongmu_lidar_imu_calib
- 就是基于浙大2020 开源代码做的修改
- 此程序有如下几个部分
  1. 初始化。
    - 通过手眼标定来标定R。其中雷达的旋转通过NDT来求。IMU的数据则是先存入B样条中，然后从B样条轨迹中提取。
    - 原来的矩阵分解方法被ceres求解代替。
    - 此代码添加了对标定数据是否系统可观的判断，也可以说是数据维度是否足够。标定的旋转矩阵自由度是三维。那么应该保证数据分解后第三大的特征值不能太小。
    - 目前代码添加了一个退出标志。当处理的数据帧数到了一个值之后，就认为数据可观性满足了，从而输出标定结果。。==【但数据可观性在整个程序中很重要。后面的求解还是需要IMU在各个轴充分转动的。或者说，需要雷达与IMU之间的相对转换关系，各个轴上的激励需要足够大。比如将IMU斜着放，是可以增加激励的】==
  2. 数据关联。
    - 根据B样条中的信息（IMU信息），对每一帧点云做旋转尺度上的矫正，然后建立NDT地图
    - 对于NDT中每个网格(voxel),判断其中是否有平面，保存平面内点，平面方程和平面类型。
    - 遍历所有帧数据与所有平面类型，找每个点对应到哪个平面及平面类型。
    - 对平面做平均下采样后(10进1)，打印有多少个有效配准。(平面点大于等于20个才算有效配准)
  3. BatchOptimization。(块优化)
    - 建立优化问题，将陀螺仪，加速度计以及点与平面对应的数据加入优化问题中，联合优化求解。打印是否收敛。
    - 此处代码封装的比较严重。主要的信息在 Kontiki 库中。想要看明白是怎么添加观测数据，设置残差，怎么设置优化问题的，得参考开发者手册： https://hovren.github.io/kontiki/developers/extending.html
    - 此处目前没有做任何修改。如果想要更适配纵目的数据集，可能需要做适当的修改，甚至调参。。
  4. Refinement(细化优化)
    - 先做数据关联，再做块优化
    - 数据关联有不一样的地方。矫正时用的是上一次标定后的R,T数据
  5. 经过多次 refinement 之后，如果平面数目趋于上升或者稳定，这说明结果趋于收敛，此时得到的结果一般是精度较高的。如果平面数据持续下降，那么说明数据维度不够或者场景内平面不够，无法配准。

附录2：浙大2020 开源代码代码文件结构

lidar_IMU_calib
- inlcude
  - core
    - scan_undistortion.h
      - 用于矫正点云帧。有两个功能，第一个是用手眼标定的R来矫正点云，第二个是在refinement过程中，用B样条中的数据来矫正点云。
  - ui
  - utils
    - ceres_func.hpp (自己写的用ceres库做手眼标定求解R)
    - dataset_reader.h
      - 重要文件。跟点云数据集有关，组织点云。见 unpack_scan 函数。
      - 原作者直接使用雷达原始数据进行解析，然后根据每个数据的垂直角与水平角，存入一个rangeimage中。
      - 我这里输入时pcd,通过手动计算每个点的垂直角和水平角，然后存入rangeimage中
- launch
  - licalib_gui.launch (一个 launch 文件)
- src
  - core
    - inertial_initializer.cpp
      - 第一部分，通过手眼标定求取R。既有矩阵分解方法，又有优化库求解方法。
      - 注意: 矩阵分解的特征值大小会用于判定输入的数据集在各个维度的激励是否足够。
      - 再注意，当前还传入了一个参数 end_flag, 用于跳过激励检测，默认激励足够，从而让程序可以进入下一级
    - lidar_odometry.cpp
      - 雷达里程计相关代码。读取雷达数据，通过NDT配准计算里程计，从而计算雷达的轨迹，存入B样条轨迹中。
      - 只提取关键帧存入地图中
      - 每隔100帧保存一次点云地图。从地图中可以看出雷达里程计的结果准不准。
    - surfel_association.cpp
      - 对每个NDT网格，判断其中是否有平面，属于哪种类型的平面
      - 判断每个网格中多少个点在该网格中的平面上。即点与平面进行关联
    - trajectory_manager.cpp
      - 与B-SPLINE 连续时间优化库对接的代码文件。负责传数据到B样条中，以及从B样条轨迹中提取数据。
      - ==极其重要，创建Bspline,插入数据，读取数据，基于Bspline添加来拟合优化器都在这里。==
  - ui
    - calib_ui.cpp (设置UI，并定义各个按钮功能)
    - calib_helper.cpp (不同按钮功能的顶层函数。用于ui与core功能对接)
- test
  - 顶层测试文件，用于启动GUI
- thidparty
  - Kontiki(Continuous-Time Toolkit,即B-spline的相关代码)
    - 这里原作者也定义了一些文件，具体怎么自己开发可以参考此库的开发人员手册。开发者手册： https://hovren.github.io/kontiki/developers/extending.html
    - ==很重要，注意看/Kontiki/include/kontiki/measurements 以及 /include/kontiki/trajectories 中的代码。这里面有如何构建联合优化器，如何提取Bspline数据的代码==
  - Pangolin(for visualization and user interface)
- calib.sh (原作者的启动程序脚本)
- calib_my.sh (用KAIST08数据集做标定的脚本))
- calib_zongmu.sh (用之前纵目数据集做标定的脚本)
- calib_zongmu_new.sh (用最新录制的纵目数据集做标定的脚本)

附录3：具体操作流程

提交的文件夹路径应该是

Lidar_Imu_Calib/src/
- imu_csv_to_rosbag
- zm_imu_filter_process
- lidar_imu_hand_eye_calib
- zongmu_lidar_imu_calib

// 根据 imu 的 .csv 数据，lidar数据 rosbag 以及他们数据的相对时间戳，生成 lidar_imu_bag.rosbag
cd Lidar_Imu_Calib/src/imu_csv_to_rosbag
code README.md // (用 vs code 查看文档，根据文档提示设置相关参数)
cd ..
catkin_make
rosrun rosbag_merge rosbag_merge_node [input_rosbag] [imu_csv] [output_rosbag]
rosbag reindex [output_rosbag]  // 将rosbag中的信息按时间做重新索引。
rosbag info [output_rosbag]  // 查看生成的rosbag包的信息

// 对第一步生成的 lidar_imu_bag.rosbag，做IMU数据互补滤波
cd Lidar_Imu_Calib/src/zm_imu_filter_process
code ReadMe.txt  // (用 vs code 查看文档，根据文档提示设置相关参数)
cd ..
catkin_make
rosrun imu_process imu_cf_node /zongmu/imu /zm/filter_imu  // 假设生成的bag为 [lidar_imu_bag_filtered.bag]
rosbag reindex lidar_imu_bag_filtered.bag
cd Lidar_Imu_Calib/src/zm_imu_filter_process/draw_euler
./run_draw.sh   // 查看滤波后的数据的角度折线图
// 也可以回放 lidar_imu_bag_filtered.bag， 通过 rviz 在 话题 odom 下观察姿态变化
// 观察姿态变化，画出大概的轨迹图，然后截取rosbag包，从而减轻后续的处理负担。 使用 rosbag filter 命令
rosbag filter KAIST_urban08.bag KAIST_urban08_trunc.bag "topic == '/ns1/velodyne_points' or topic == '/imu/data_raw'"
rosbag filter KAIST_urban08_trunc.bag KAIST_urban08_trunc2.bag "t.to_sec() >= 1628047360.00 and t.to_sec() <= 1628047480.00"

// 用互补滤波后的数据 lidar_imu_bag_filtered.bag ，做手眼标定，求出旋转矩阵。
cd Lidar_Imu_Calib/src/lidar_imu_hand_eye_calib/src/lidar_imu_calib-main/
code README.md // (用 vs code 查看文档，根据文档提示设置相关参数)
cd ..
catkin_make -DCATKIN_WHITELIST_PACKAGES="ndt_omp;lidar_imu_calib"
cd src/lidar_imu_calib-main/lidar_imu_calib/launch
do: config the launch file; the lidar_topic, the imu_topic and the bag_file
code Lidar_Imu_Calib/src/lidar_imu_hand_eye_calib/src/lidar_imu_calib-main/lidar_imu_calib/src/lib/calibExRLidar2Imu.cpp // 查看并配置相关路径
roslaunch lidar_imu_calib zongmu_calib_exR_lidar2imu.launch // 运行程序
cd cd Lidar_Imu_Calib/src/lidar_imu_hand_eye_calib/src/lidar_imu_calib-main/lidar_imu_calib
./run_compare_hand_eye_lidar_imu.sh     // 比较观察手眼标定结果是否正确
./run_compare_lidar_imu.sh              // 比较观察 lidar imu 的角度，参考意义不大

// 用互补滤波后的数据 lidar_imu_bag_filtered.bag ，跑浙大2020 IROS 代码
cd Lidar_Imu_Calib/src/zongmu_lidar_imu_calib/src/lidar_IMU_calib
code README.md // (用 vs code 查看文档，根据文档提示安装编译)
code README_code.md // (用 vs code 查看文档，查看整个代码的架构)
code calib_zongmu.sh  // 根据自己需要修改运行参数
code src/ui/calib_helper.cpp  // 根据数据包需求设置 MAX_LIDAR_SIZE 参数
./calib_zongmu.sh       // 运行程序后，出现可视化界面
// 先点击 ， 再点击 ， 最后一直点 ， 直至平面个数等不再变化。

附录4： B样条资料

https://blog.csdn.net/qq_40597317/article/details/81155571