多激光雷达外参⾃动化标定算法及代码实例
激光雷达是⽬前⾃动驾驶系统中的核⼼传感器之⼀，但是由于其信息密度低、存在垂直盲区等问题，⼚商⼤多在其L4级⾃动驾驶系统中搭配多组激光雷达，下图为通⽤（Cruise）的⾃动驾驶汽⻋，采⽤了多激光雷达以弥补lidar+camera的不⾜，使⽤多激光雷达进⾏环境感知的前提是对各雷达的外参进⾏精准的标定，本⽂介绍⼀种基于NDT算法的⾃动多激光雷达标定技术，并且给出了代码实例以及测试数据（rosbag）供读者实践。
NDT算法详解
传感器外参标定本质上是获得两个传感器的位移量（x，y，z ）和旋转量（roll，pitch，yaw ），三维空间中可以⽤⼀个⻬次变换矩阵（Homogeneous transformation matrix）来描述这样的变换关系，在三维数据处理领域，点云配准（Point Cloud Registration）就是⽤于处理两个点云间位姿匹配问题的⼀类x, y, z roll, pitch, yaw⽅法，其中NDT(Normal Distribution Transform，正态分布变换)和ICP(Iterative Closest Point, 迭代最近点算法)是其中的代表。本⽂主要基于NDT算法介绍多激光雷达外参标定的⽅法和实践。
其中，α 为yaw ，β 为pitch ， γ为roll 。
假定ND体素k中包含有m个点，那么这个ND体素中所有点的均值μk和协⽅差矩阵Σk计算公式为:
其中pki为ND体素k中的点i，即
，那么该ND体素内点的概率密度函数f(k)可以表示为：
NDT匹配的准确度和ND体素格的划分尺⼨相关，采⽤的ND体素尺⼨越⼩，相应的NDT匹配精度也会越⾼，所以在利⽤NDT算法进⾏扫描匹配定位时需要对匹配精度和算法实时性间进⾏取舍，但是在本例中，我们使⽤NDT进⾏多激光雷达标定为离线任务，所以可以将ND Voxel设置相对⼩⼀些以提⾼最终的精度。
NDT扫描匹配算法
,旋转参数向量表⽰变换后的点云相对当前输⼊点云在姿态⻆上的旋转量，平移参数向量表⽰在(x, y, z)三个⽅向的平移量。那么对于输⼊点云⽽⾔，每个点进⾏三维坐标转换的计算公式如下：
NDT算法⾸先对⽬标点云进⾏正态分布变换，得到⽬标点云的所有ND体素，接着需要输⼊当前输⼊点云的初始位姿（x, y, z, roll, pitch, yaw），将此位姿作为初始搜索位置。在传感器标定中，这个初始变换位姿也就是我们对于多个激光雷达平移量和旋转量的粗略估计，可以通过简单的测量得到（精度不需要太⾼，我们最终将通过NDT算法得到⾼精度的结果）。
其中，
是输⼊点云在经过三维坐标变换参数θ转换后的点，μi是与该输⼊点相对应的⽬标点云ND体素格的均值，Σi是相应的ND体素内的协⽅差矩阵。拟合度F itness(P , θ)数值越⼤说明输⼊点云和⽬标点云在该位置越匹配，通过搜索参数向量 得到⼤的拟合度，这是⼀个典型的⾮线性最优化问题，NDT算法使⽤⽜顿法来求解最优参数。⽜顿法是⼀种最⼩化⽬标函数的⽅法，所以⽬标函数f (θ)为：
其中θ = (α, θ, γ, dx, dy , dz )T，是输⼊点云到⽬标点云的三维坐标转换参数。通过迭代⽜顿法，不断调整θ向量，使得f (θ)⼩于⼀个阈值，则称NDT参数优化已经收敛，根据此时的变换参数向量θ即可确定输⼊点云在⽬标点云中的姿态，也就是lidar A到lidar B的TF关系。
多激光雷达标定代码实例
在理解NDT算法后，我们知道这类基于点云配准的多激光雷达标定⽅法需要给⼀个初始姿态估计，这个初始估计要求的精度不⾼，可以简单的测量甚⾄估计得到，下载后台提供的rosbag和代码仓库，通过rosbag info我们发现该bag包含以下数据：
这个bag包含了6个激光雷达的数据，我们选取其中⼀个Lidar作为主要的坐标系（TF中的parentframe），建⽴起其他5个lidar到它的TF变换关系，就完成了6激光雷达的外参标定。选取 /hesai/front_high 作为parent frame，我简单的⽬测估计了其他5颗激光雷达到该雷达的姿态变换关系并且写到了项⽬的 cfg/child_topic_list ⽂件中：
voxel_size ：输⼊点云的降采样尺度，在执⾏NDT算法之前我们通常会对输⼊点云进⾏预处理，如体素降采样，以加速计算; ndt_epsilon ：该参数定义了搜索的最⼩变化量，该参数尺度过⼤会造成最终的收敛不稳定，过⼩容易造成收敛速度过慢的问题; ndt_resolution ：⽬标点云的ND体素的尺⼨，单位为⽶; ndt_iterations ：使⽤⽜顿法优化的迭代次数，迭代次数越多计算量越⼤。
理解好参数以后简要的过⼀下代码，本⽂实例主要基于PCL库，在主函数中，我⽤了⼀个频率为10Hz的loop：
对应的回调 PointsCallback 中我们并没有写代码的业务逻辑，仅仅只是获取点云数据并保存于内部成员变量中，并对输⼊点云做了Voxel Grid降采样：
点云的配准我们写在了循环中的 PerformNdtOptimize() 函数中,⾸先判定成员变量是否已经接受到点云消息,接着初始化NDT的参数、输⼊点云和⽬标定义：
如果没有初始估计，则从配置⽂件中读取位移量和欧拉⻆换算成⻬次变换阵：
执⾏NDT点云配准，得到新的变换矩阵，基于新的变换矩阵对输⼊点云进⾏位姿调整，并且发布该debug点云以在rviz中可视化：
提⽰：你需要安装catkin_tools才能够使⽤ catkin build 指令进⾏编译，你可以选择：
1. 安装catkin_tools：
sudo apt-get install Python-catkin-tools
2. 或者使⽤ catkin_make 进⾏编译。
编译完成后启动roscore:
roscore
在另⼀个终端中启动rviz（使⽤我提供的rviz配置⽂件）：
rviz -d rviz/multi_lidar_calibration.rviz
运⾏
通过Rviz可以看到⼤概⽤时不到5秒中左右，两颗激光雷达即标定完成，如下图：
并且可以得到相应的TF关系，如下图：
程序的输出：
将收敛后程序的输出结果中的1.00938 -0.478343 -0.442721 1.36447 0.0686235 -0.080712 /fh /lf 10 拷⻉到 ⽂件launch/tf.launch中的args中，我们可以通过ros tf中的static_transform_publisher将6颗激光雷达的 数据融合在⼀个坐标系下。
修改multi_lidar_calibrator.launch中的points_child_src名称为其他data的topic，执⾏程序5次即可完成所有 lidar的标定，得到5组外参（⻬次矩阵、TF、平移量和四元数均能得到），将结果添加到tf.launch⽂件 中后，我们就可以通过Rviz看到6颗激光雷达的标定结果了，执⾏tf.launch并运⾏bag：
rosbag play multi_lidar.bag -l –clock
