多激光雷达外参⾃动化标定算法及代码实例

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

激光雷达是⽬前⾃动驾驶系统中的核⼼传感器之⼀，但是由于其信息密度低、存在垂直盲区等问题，⼚商⼤多在其L4级⾃动驾驶系统中搭配多组激光雷达，下图为通⽤（Cruise）的⾃动驾驶汽⻋，采⽤了多激光雷达以弥补lidar+camera的不⾜，使⽤多激光雷达进⾏环境感知的前提是对各雷达的外参进⾏精准的标定，本⽂介绍⼀种基于NDT算法的⾃动多激光雷达标定技术，并且给出了代码实例以及测试数据（rosbag）供读者实践。

Cruise的多激光雷达传感器配置

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算法介绍多激光雷达外参标定的⽅法和实践。

⻬次变换矩阵：描述两个坐标系间的平移和旋转变换关系的4×4矩阵，给定两个坐标系的平移量(xt, yt , zt )和欧拉⻆R(α, β, γ) ，⼀个3D的⻬次变换矩阵写作：

其中，α 为yaw ，β 为pitch ， γ为roll 。

正态分布变换

正态分布变换是⼀种⽤正态分布函数来描述⼀个体素⽹格（voxel）内的点的⽅法，令P = {pi∣i =0, 1, ..., t − 1} = {(xi, yi , zi )∣i = 0, 1, ..., t − 1}表示⼀个包含 个点的点云，NDT算法⾸先使⽤三维的⽹格（也称为体素）将点云进⾏划分，如下图所⽰，我们称这类体素为ND体素：

假定ND体素k中包含有m个点，那么这个ND体素中所有点的均值μk和协⽅差矩阵Σk计算公式为:

其中pki为ND体素k中的点i，即，那么该ND体素内点的概率密度函数f(k)可以表示为：

按照此⽅法，可以将整个⽬标点云使⽤ND体素进⾏划分，并且计算每⼀个ND体素内的正态分布参数。

NDT匹配的准确度和ND体素格的划分尺⼨相关，采⽤的ND体素尺⼨越⼩，相应的NDT匹配精度也会越⾼，所以在利⽤NDT算法进⾏扫描匹配定位时需要对匹配精度和算法实时性间进⾏取舍，但是在本例中，我们使⽤NDT进⾏多激光雷达标定为离线任务，所以可以将ND Voxel设置相对⼩⼀些以提⾼最终的精度。

题外话：在利⽤NDT继续激光雷达扫描匹配定位中，考虑到城市道路场景下地图的尺⼨通常较⼤，采⽤⾼密度的地图尺⼨进⾏划分将造成内存占⽤偏⾼的问题，并且使得匹配的计算量增⼤，⽆法满⾜定位的实时性要求。所以在⾃动驾驶定位的场景中，通常使⽤较⼤尺⼨的ND体素格配合⾼密度的点云地图来平衡NDT匹配定位的实时性和准确性。

NDT匹配参数

NDT匹配定位算法定义了⼀些匹配参数，通过这些匹配参数可以将输⼊点云进⾏三维坐标变换，得到输⼊点云到⽬标点云中的变换关系，注意由于本⽂讨论的是激光雷达的外参标定，所以⽬标点云实际上就是⽬标激光雷达（ROS TF中的parent frame），输⼊点云就是要标定到⽬标激光雷达坐标系下的激光雷达的点云（ROS TF中的child frame）。NDT算法中⼀共有两组配置参数，分别是旋转参数向量(α, θ, γ)T和平移参数向量,旋转参数向量表⽰变换后的点云相对当前输⼊点云在姿态⻆上的旋转量，平移参数向量表⽰在(x, y, z)三个⽅向的平移量。那么对于输⼊点云⽽⾔，每个点进⾏三维坐标转换的计算公式如下：

其中R为旋转矩阵，其计算公式为：

所以对于NDT匹配算法⽽⾔，配准的⽬标就是找到⼀组三维坐标系变换参数向量，使得输⼊点云在经过这组参数指定的旋转和平移之后，能够和⽬标点云拟合。我们称参数向量为NDT匹配参数，接下来，将介绍如何搜索找到这⼀组参数。

NDT扫描匹配算法

NDT算法⾸先对⽬标点云进⾏正态分布变换，得到⽬标点云的所有ND体素，接着需要输⼊当前输⼊点云的初始位姿（x, y, z, roll, pitch, yaw），将此位姿作为初始搜索位置。在传感器标定中，这个初始变换位姿也就是我们对于多个激光雷达平移量和旋转量的粗略估计，可以通过简单的测量得到（精度不需要太⾼，我们最终将通过NDT算法得到⾼精度的结果）。

初始位姿是对初始点云在⽬标点云中的位姿的估计，这个估计值可以帮助NDT算法中的参数优化⽅法迅速收敛。得到初始估计后，开始搜索⼀组最优的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 ⽂件中：

第⼀⾏是child frame的个数（即其他激光雷达的个数）。之后依次是每个lidar的点云数据topic名称以及到主雷达变换的初始估计（x, y, z, yaw, pitch, roll），为了让读者感受到NDT算法收敛的过程，我有意将初始估计做的⾮常差（平移量存在1⽶左右的误差，旋转量普遍存在20-30度的误差）。接着是launch⽂件中的参数：

这⾥的 points_parent_src 即主雷达的topic， points_child_src 是这⼀次标定的雷达的topic，每执⾏⼀次程序可以标定两颗雷达的外参，对于本例⽽⾔，执⾏5次程序即可⾃动完成所有雷达的标定，接着是NDT算法的配置参数：

• voxel_size ：输⼊点云的降采样尺度，在执⾏NDT算法之前我们通常会对输⼊点云进⾏预处理，如体素降采样，以加速计算;

• ndt_epsilon ：该参数定义了搜索的最⼩变化量，该参数尺度过⼤会造成最终的收敛不稳定，过⼩容易造成收敛速度过慢的问题;

• ndt_resolution ：⽬标点云的ND体素的尺⼨，单位为⽶;

• ndt_iterations ：使⽤⽜顿法优化的迭代次数，迭代次数越多计算量越⼤。

理解好参数以后简要的过⼀下代码，本⽂实例主要基于PCL库，在主函数中，我⽤了⼀个频率为10Hz的loop：

我们没有通过callback来调⽤代码逻辑，⽽是采⽤了固定的循环，这样我们的rosbag就不需要很⼤（哪怕每个topic只包含⼀条消息也能收敛），在 InitializeROSIo 函数中主要初始化初始估计以及NDT参数，并且使⽤message_filters同步两个topic的消息，代码如下：

对应的回调 PointsCallback 中我们并没有写代码的业务逻辑，仅仅只是获取点云数据并保存于内部成员变量中，并对输⼊点云做了Voxel Grid降采样：

点云的配准我们写在了循环中的 PerformNdtOptimize() 函数中,⾸先判定成员变量是否已经接受到点云消息,接着初始化NDT的参数、输⼊点云和⽬标定义：

如果没有初始估计，则从配置⽂件中读取位移量和欧拉⻆换算成⻬次变换阵：

执⾏NDT点云配准，得到新的变换矩阵，基于新的变换矩阵对输⼊点云进⾏位姿调整，并且发布该debug点云以在rviz中可视化：

根据求得的⻬次变换矩阵换算TF，使⽤TF Broadcaster发布该TF：

使⽤测试数据实践6激光雷达标定

下载后台的数据和代码，将代码cp到你的ros workspace的src⽬录下，编译：

catkin build

提⽰：你需要安装catkin_tools才能够使⽤ catkin build 指令进⾏编译，你可以选择：

1. 安装catkin_tools：

sudo apt-get install python-catkin-tools

2. 或者使⽤ catkin_make 进⾏编译。

编译完成后启动roscore:

roscore

在另⼀个终端中启动rviz（使⽤我提供的rviz配置⽂件）：

cd src/multi_lidar_calibration

rviz -d rviz/multi_lidar_calibration.rviz

运⾏

source devel/setup.bash

roslaunch multi_lidar_calibrator multi_lidar_calibrator.launch

rosbag play multi_lidar.bag

通过Rviz可以看到⼤概⽤时不到5秒中左右，两颗激光雷达即标定完成，如下图：

并且可以得到相应的TF关系，如下图：

程序的输出：

rostopic echo /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：

roslaunch multi_lidar_calibrator tf.launch

rosbag play multi_lidar.bag -l --clock 

结果如下：

后台回复“rosbag”获取代码和测试数据（rosbag），考虑到⼤家下载的困难，本例的bag做了处理只有300多MB。

作者简介：

申泽邦，奔驰高级自动驾驶工程师，Google Developer Expert（机器学习方向），兰州大学自动驾驶团队创始人，CSDN博客专家，《无人驾驶原理与实践》书籍作者。

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近3000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~