基于固态激光雷达辅助的十六线机械雷达和单目相机的外参标定方法

专注SLAM、点云、三维重建、结构光、自动驾驶、机械臂抓取等

激光视觉联合标定的目的是构建激光点云和视觉像素点之间的关系，通过标定得到的激光雷达和相机之间的外参把三维的激光点投影到相机坐标系下，然后利用相机的模型把三维点投影到像素平面。由于标定的结果直接影响信息融合的效果，所以标定技术是多传感器之间信息交互中的关键。

作者：Liam | 来源：微信公众号「3D视觉工坊」

标定的关键是寻找场景中激光点云和图像平面的对应点，利用对应点求出相机和激光雷达之间的外参。对于2D的像素点和3D的激光点，所以这个问题可以构建为如公式所示。

(1-1)

公式左边表示激光在图像坐标系中的像素的齐次坐标，右边第一个矩阵为相机的内参矩阵，相机的针孔模型中的参数；第二个矩阵表示激光雷达到相机的外参，主要是旋转矩阵R和平移向量t；最后一个向量表示激光点在激光坐标系下的齐次坐标。

由于机械激光雷达单帧的点云稀疏，直接基于机械点云和图像匹配构建匹配关系比较困难，本课题引入具有非重复扫描特性的稠密的固态激光雷达点云作为中间标定过程。首先利用固态激光得到的场景的稠密点云，并基于如表所示的方法提取场景中的线特征，然后再视觉图像中基于成熟的线段检测器(Line Segment Detector, LSD)提取线特征，最后基于激光点云和图像中的线特征进行数据关联得到准确的固态雷达和相机之间的外参标定结果。

表1：基于体素的边缘提取算法

检测出来的激光的线特征和视觉的线特征匹配，对于每一个激光点云中的线特征，采样线上的点，每一个采样点通过当前估计出来的外参投影到相机平面。

(1-2)

利用刚体变换可以得到激光点在相机坐标系下的位置，然后利用第二节介绍的针孔模型π和相机的畸变模型f把相机坐标系下的激光投影到点投影到图像平面上，并得到去畸变后的激光点在图像上的位置。

(1-3)

利用图像中的线特征像素构成的kd-tree找到离当前投影点最近的若干个点，可以得到一个集合Q，计算最近像素点的均值和方差。

(1-4)

最后利用图像线特征上的一点q和协方差矩阵的最小特征值对应的特征向量得到线特征垂线方向n得到线特征的数学表达式。同时验证激光点云投影到图像上的线特征和计算得到的n的正交性来快速的去除距离很近但是又不平行的错误匹配。利用激光点云中检测到的线特征上的一个点LPi ，激光线特征匹配的图像线特征的法向量ni及激光点云线特征对应的图像线特征上的一点qi构建残差函数。

(1-5)

得到残差方程和优化变量后就可以基于高斯牛顿或者其他的非线性优化方法得到最优的固态激光雷达和相机的外参。

得到了准确的相机和固态雷达的外参之后，下一步就是做机械雷达和固态雷达之间的外参Tlil标定，同样的在静态环境下采集点云信息，分阶段的利用点云信息优化外参，首先根据前边介绍的NDT对齐输入的两帧点云得到外参的初值，然后检测两帧点云中的线特征和平面特征，构建误差函数进一步优化得到准确的机械雷达和固态雷达之间的外参。最后利用得到的固态和相机之间的外参Tcl1和机械雷达和固态雷达之间的外参Tlil就可以得到需要的相机和机械雷达之间的外参Tcl。

(1-6)

基于固态激光雷达的稠密点云对机械雷达和ZED相机的左目进行标定，相机、固态激光雷达和16线的机械雷达采集的数据如下图所示。

(a) 机械激光雷达点云 (b) 固态激光雷达点云 (c) 图像信息

传感器采集信息示意图

图 (a)为机械雷达采集得到的点云，可以看到单帧激光点包含的信息很少很难直接和图像对齐，而 (b)为固态激光雷达重复扫描得到的场景信息，可以看到场景中丰富的结构信息，较为容易和(c)所示的视觉信息对齐。

首先利用固态激光得到的场景的稠密点云和视觉图像基于线特征进行标定，首先基于表1提出的方法提取固态激光点云中的线特征，然后利用LSD提取视觉图像中的线特征。

(a) 固态激光点云线特征 (b) 图像线特征激光点云与图像线特征提取图

由于不同方向的线特征对解算位姿不同的方向约束不同，所以寻找线特征较丰富且方向较多的场景，本文选择基于图书馆外观中丰富的线特征进行实验。如图(a)所示为固态激光雷达点云中提取的线特征，图(b)为图像中丰富的线特征。利用最近邻搜素的方式匹配激光点云和图像中的线特征，然后求解最优相机和固态激光雷达间的外参，利用迭代优化后的外参把激光点云投影到图像上结果如图所示。

固态激光点云和图像对齐结果

得到相机和固态激光雷达的外参后，利用固态激光雷达点云和机械雷达点云利用icp或者其他的方法进行数据关联，得到准确的外参，利用该外参把固态激光点云和机械雷达点云对齐后如下所示。

固态激光点云和机械雷达点云对齐结果

其中红色点表示原始的机械雷达点云，蓝色点表示对齐后的机械雷达点云，绿色点表示稠密的固态激光雷达点云，其中对齐的边缘细节如图所示。

固态激光点云和机械雷达点云对齐细节

基于固态雷达与相机的外参及固态雷达与机械雷达的外参就可以得到相机和机械雷达准确的外参，利用该外参把机械雷达点云投影到图像上如图所示。

机械激光雷达点云和图像对齐结果

参考文献及代码：

固态-单目相机标定：https://github.com/hku-mars/livox_camera_calib

论文在工坊分享过：针对高分辨率雷达和相机的无标定板的像素级外参自标定方法。

激光点云数据关联方法：

https://github.com/ethz-asl/robust_point_cloud_registration