车端激光和双目相机的自动标定算法

作者：Liam，原文：车端激光和双目相机的自动标定算法
注1：文末附有【视觉SLAM、激光SLAM】交流群加入方式
注2：自动驾驶系统课程：国内首个面向自动驾驶领域的多传感器融合系统学习课程
注3：彻底搞懂视觉-惯性SLAM：基于VINS-Fusion
注4：彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
注5：彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM+LIO-SAM)
注6：激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解

在车端配置一个双目相机再加上一个激光雷达已经成为一种比较常用的配置。但是为了融合相机和激光数据我们需要一个复杂的标定过程。本文提供了一种不需要人干预的自动化的双目和激光雷达的外参估计方法。本文的标定方法是克服在自动驾驶车辆中的传感器的常见的限制，如低分辨率和特殊的传感器的位置（如你在车端在没有升降台的情况下不能让车上下动，roll,pitch旋转）。为了证明算法的可行性，作者分别在仿真和真实环境中做的实验都表现出了比较好的结果。

高级驾驶辅助系统（ASAS, Advanced Driver Assistance Systems）或者是自动驾驶的系统需要准确的环境感知来正确的进行路径规划，在可能影响驾驶方法的情况下（例如光照的变化），这些系统必需要有一定程度的鲁棒性。因此车端配置的传感器需要可以在不同的情况下互补。视觉系统可以提供场景的外观信息，所以在车端系统中很常见。双目视觉能提供一定范围内的场景的深度信息，所以作为一种经济高效的传感器，可以提供车周围一定范围内稠密的点云信息。而激光雷达的特点是测量精度高（测量飞行时间，根据点的线束和角度信息投影到空间坐标系），而且可以提供360度范围内的点云信息。(但是相机的深度信息毕竟不准确，但是分辨率高；激光的测量虽然准，但是分辨率很低，特别是z轴上，造成纯激光的系统都会有很大的z轴漂移）。

由于激光和视觉都有他们的缺陷和优势，所以两个传感器可以融合起来。但是我们融合的时候，一个精准的外参就很重要了。（比如我们要利用得到像素点的深度，需要利用外参把单帧或者多帧激光点云利用外参投影到图像上获得像素点的深度，如果外参不准，我们深度恢复的结果也很难准。还有一些语义信息的投影诸如此类的融合都需要一个比较准的外参）。

当前外参估计的方法可能需要复杂的过程或者缺乏泛化能力，标定的精度完全取决于标定的环境或者初值的准确程度。本文提出了一种针对车端双目相机和激光雷达的标定的方法，和现有的方法不同，本文没有对系统提出任何强有力的假设，可以标定16线这种低分辨率的激光雷达，也可以允许传感器之间的位姿很不同。本文的方法只是假设两个融合的数据没有很大的时间间隔，这是很容易做到的（激光一般是10HZ，相机是30HZ）。

二、相关工作

在过去几年，关于不同的传感器的外参标定的工作很多，主要是自动驾驶和多传感器融合的机器人。这其中最多的研究就是关于相机和激光雷达的，由于移动平台的固有限制，比如传感器智能采集到一个或者四个面（东西南北，上下就看不到）。标定的过程一般是在传感器运行正常的人工布置的系统中执行的。传统的方法一般会需要人工标注：https://github.com/MegviiRobot/CamLaserCalibraTool（这是2007年IROS的方法，Extrinsic Self Calibration of a Camera and a 3D Laser Range Finder from Natural Scenes），然而由于机器人平台经常出现标定的错误，所以这个过程自动进行最好。

标定的过程是需要找图像平面和激光点云在不同视角下的对应点，所以一般都会用一些特殊的标定板作为目标，例如三角板，多边形或者球体。现有的方法中基于物体形状的多样性来找到他们在不同传感器的位置，但是尽管如此，场景中的平面很普遍，因为可以利用深度信息和提供的特定的兴中基于几何的计算很容易检测出来。另外，Scott等人基于运动跟踪的方法也解决了没有共视关系的传感器之间的标定的问题（Exploiting Known Unknowns: Scene Induced Cross-Calibration of Lidar-Stereo Systems， 2015年IROS）。

近些年来稠密的3D传感器发展很快（例如扫描仪，非重复扫描特性的固态激光雷达）。所以有一些研究这种稠密3D传感器的标定的工作。Automatic Cameraand Range Sensor Calibration using a single Shot（2012. ICRA）把几块标定板当做目标位置，提出了one-shot的方法。Calibration of RGB Camera With Velodyne LiDAR（2014）提出了单一视角下基于标定区域圆形特征的标定方法。

第二种方法是不急于人工设计的标定目标，而是利用环境中的特征。Line-based Extrinsic Calibration of Range and Image Sensors（2013,ICRA）这篇论文利用场景中的线段特征来计算传感器的外参。这种方法适应于室内有大量线特征的场景。在室外的道路场景，利用地面和一些障碍物可以相机和激光的外参。

另外，如何评价校准方法的准确程度也是一个没有被解决的问题。因为在实际标定的时候我们很难得到传感器之间的外参的真值。Automatic Online Calibration of Cameras and Lasers中提出了一种方法，基于场景的不连续性来计算目标函数的变化得到外参的误差。

三、标定算法

本文提出的标定方法标定的是双目相机之间的外参和相机到激光雷达的外参。如下图所示：

相机和激光雷达之间的刚体变换可以表示为：

其中前三个表示两个坐标系之间的平移，后三个表示的就是两个坐标系之间的旋转（RPY）。有了这个外参之后我们可以很容易得到相机到激光的外参的变换矩阵形式，利用这个矩阵我们可以很容易把相机系下的点转换到激光坐标系下。为了得到外参，作者采集了很多双目图像（时间同步后的）和激光帧的扫描，系统中假设相机的内参是已知的，图片是经过畸变矫正的。

本文只用了一个单个的校准目标，而且他只能在特定的视角下才可以观测到。这样可以避免在标定过程中的需要处理多个标定目标或者场景的变化。作者利用Calibration of RGB Camera With Velodyne LiDAR论文中的思路，在一个平面上挖了四个对称的圆孔，这些孔作为相机和激光雷达可见的明显特征。因此在我们放置激光和视觉的时候两个传感器的视野内要有一定的重叠，以便圆形孔与至少两个激光雷达光束相交，并且从相机完全可见。

整个系统除了上述假设外没有任何其他假设，没有对两个传感器的旋转和平移有什么要求。标定的过程分为两个阶段：两个点云（图像三角化出来的和激光扫描的）的参考点分割；利用点云对齐得到外参。

A. 数据表示

本文提出的方法是基于要标定的传感器的，利用3D激光雷达我们可以表示点云为（x,y,z）的集合。双目相机得到的信息主要利用两个：灰度图和深度信息。其中深度信息是利用双目匹配的算法，可以得到稠密的3D点云信息。在标定过程中利用两个点云的相似性来计算两个传感器之间的外参。激光的点云集合表示为P^l_0，相机的点云表示为 P^c_0。对于双目的匹配，本文利用的是SGM算法，这种方法可以得到比较准的深度估计的结果。其中边界定位的问题可以很好的利用反射信息解决。利用有一定的纹理的标定板来解决双目匹配的问题。利用滤波器和距离信息可以很好的处理点云，保证处理后的点云来自两个传感器共视的部分来更好的做标定。

B. 目标分割

标定算法的第一步是提取标定板上不连续的点。所以针对来自激光雷达和双目相机的点云执行几个连续分割的过程。分割的第一步是找到点云的表示集合形状的点集。也即是找到标定板上点。利用标定板是一个平面的优点，我们直接在原始点云中执行平面检测的算法。有一些严格的约束来保证我们找到的平面确实是标定板的平面。第一是在执行RANSAC时δ plane的选取是很严格的；第二是检测出来的平面需要和传感器坐标系中的Z轴平行（角度差在一个很小的范围内）。

当我们找到了标定板的平面后，点和平面的距离大于阈值将被剔除，这样我们就得到了平面上的点云。如图3a和图4a。然后是利用滤波的方法去除不是不连续点的点云，由于两个传感器采集到的点云的密度不同，这个过程是不同的。首先对于激光点云，本文主要参考Automatic Online Calibration of Cameras and Lasers中的方法来找到深度不连续的点。对于每一个在标定板上的点，通过计算他和相邻点的深度的差值来得到一个表示：

这表示第i个点和他左边和右边点深度差值的最大值，利用计算得到的值我们可以剔除所有连续的点，如图3b所示。而对于双目相机的点云，我们保留投影到强度图像边缘的点，然后基于Sobel滤波器在双目的左目进行滤波，在计算得到的Sobel图像中对应值比较低的点被剔除，如图4b所示。

C. 圆检测

最后一个步骤是从标定板上找到三个圆在的位置，圆的圆心会用来当做关键点做两帧点云的对齐工作。为了提升圆检测的准确度，本文进行了一个滤波的操作来去除不属于圆的点。根据不连续性已经把标定板上的点基本滤干净了，但是标定板边缘的点还存在。对于激光点云，只保存线上有圆的那些线束的点，然后在滤除环中的一些外点。对于双目相机的点云，由于标定板的边界被很密集的表示，所以我们可以基于线段来消除。利用采样一致性检测找到线段，并根据线段的方向和标定板的尺寸选择正确的是边缘的线段，以防从圆中删除有用的信息。实验表明了作者的边界滤波的方法很好，能够有效的滤除边缘线段。经过滤除边缘后的表示如图3c和4c表示。

现在相机和激光雷达的基于点表示的标定板上的圆我们已经可以得到了。最后在相机和激光雷达扫到的标定板的2D平面上进行圆的检测。把第二阶段得到的点云投影到平面上，然后基于平面方程的约束找到在平面上的点。然后在XY平面上根据已知的圆的半径基于抽样一致性检测来检测圆。为了避免检测到错误的结果。作者把标定的传感器到平面的距离也做为了一个约束。最后把检测到的中心投影到3D的空间中。我们就可以得到图3d和4d。这里需要注意的是由于圆的分割是在二维空间进行的，所以这里的传感器需要分辨率稍微高一点（论文中说16线的就可以），平面上的圆可以由三个点确定，所以需要两个雷达的线穿过每个圆（这样就最少有四个点在圆上）。

如果标定板在两个传感器的表示足够准确，我们就可以基于检测得到的四个圆心进行配准。同时为了提升对过程中噪声的鲁棒性，圆心在n帧的窗口上累积。然后利用聚类算法在点云配准阶段找到点云的质心。所以本文的方法要求在标定的整个过程标定系统都保持静止。本文基于欧几里得距离的聚类策略设定一定的阈值来进行聚类来剔除异常值对系统的影响。根据窗口长度的约束，可以最每个点集中点的最大值和最小值进行严格的约束。

最后一步的匹配通过最小化聚类的圆的中心的参考点的距离来得到相机和激光雷达的外参。匹配的过程主要分为两个部分：在假设没有旋转的情况下求最优的变换矩阵，这其实就是在计算两个点云之间的平移，可以通过一个三维的向量来表示三个轴的平移。通过四个参考点得到12个方程基于最小二乘的可以求出三个轴上的平移。

激光系下和相机系下每对点可以构建三个方程，点通过他在标定板的不同位置来命名：top-left (tl), top-right (tr), bottom-left, (bl) and bottom-right (br)。最后基于QR分解找到方程的最小二乘解。最后是匹配的过程，这个过程估计平移和旋转（xyz rpy）。作者直接基于A method for Registration of 3-D Shapes的方法来最小化距离点之间的距离。

作者分别在仿真环境和真实环境中做了精度评价的实验。作者基于Gazebo环境中配置了九种不同的传感器设置，具体的摆放如下表所示：

仿真中用的传感器是一个双目相机+16线的激光雷达，评估出来的精度：

作者也在实际场景中了实验，由于无法采集真值，所以智能通过投影的效果来判断标定的效果：

仿真环境搭建：

• 下载仿真环境的代码：https://github.com/beltransen/velo2cam_gazebo，如果你用的是ubuntu18.04则需要更新/velo2cam_gazebo-master/gazebo_plugins/velodyne_gazebo_plugin中的内容，利用https://bitbucket.org/DataspeedInc/velodyne_simulator/src/master/中对应文件夹中的文件替换。
• 把/velo2cam_gazebo-master/gazebo_models文件夹中文件移到.gazebo/models
• 直接利用launch文件进行仿真，例如：roslaunch velo2cam_gazebo mono_hdl64_p1_real.launch ，这里正常情况下会报一大推找不到模型的错误。
• 我们需要在https://github.com/osrf/gazebo_models找对应的model下载，但是好像缺的太多了，我就很暴力的把他们都下载了下来，大家可以在百度云下载，然后把他们放到.gazebo/models，再运行就好了（这里在launch文件中gui是false，我们需要把他改为true才会自动打开gazebo）：链接: https://pan.baidu.com/s/1J0ftVrmewjYeLdCQZ4wqpg 提取码: nfgx 。

运行结果如下：

我们可以根据算法得到的结果和真值比较判断算法的精度。

[1] Guindel C , J Beltrán, D Martín, et al. Automatic Extrinsic Calibration for Lidar-Stereo Vehicle Sensor Setups[J]. IEEE, 2017.

欢迎加入【3D视觉工坊】交流群，方向涉及3D视觉、计算机视觉、深度学习、vSLAM、激光SLAM、立体视觉、自动驾驶、点云处理、三维重建、多视图几何、结构光、多传感器融合、VR/AR、学术交流、求职交流等。工坊致力于干货输出，为3D领域贡献自己的力量！欢迎大家一起交流成长~

添加小助手微信：CV_LAB，备注学校/公司+姓名+研究方向即可加入工坊一起学习进步。