MATLAB - 激光雷达 - 相机联合标定（Lidar-Camera Calibration）

系列文章目录

前言

一、

激光雷达 - 相机标定建立了三维激光雷达点和二维相机数据之间的对应关系，从而将激光雷达和相机输出融合在一起。

激光雷达传感器和相机被广泛用于自动驾驶、机器人和导航等应用中的三维场景重建。激光雷达传感器捕捉环境的三维结构信息，而相机则捕捉色彩、纹理和外观信息。激光雷达传感器和相机各自根据自己的坐标系捕捉数据。

激光雷达 - 相机标定包括将激光雷达传感器和相机的数据转换为同一坐标系。这样就可以融合两个传感器的数据，准确识别场景中的物体。该图显示了融合后的数据。

激光雷达-相机标定包括内参标定和外参标定。

内参标定 - 估算激光雷达传感器和相机的内部参数。制造商会事先标定激光雷达传感器的内参参数。您可以使用 estimateCameraParameters 函数来估算相机的内在参数，如焦距、镜头畸变和偏斜。有关详细信息，请参阅单相机标定示例。您还可以使用 Camera Calibrator 应用程序以交互方式估算相机参数。
外参标定 - 估计激光雷达传感器和相机的外部参数，如位置、方向，以建立传感器之间的相对旋转和平移。

1.1 激光雷达和相机的外参标定

激光雷达传感器和相机的外参标定估算它们之间的刚性变换，以建立它们坐标系之间的几何关系。这一过程使用标准标定对象，如带有棋盘图案的平面板。

该图显示了使用棋盘格对激光雷达传感器和相机进行外参标定的过程。

外参标定的程序化工作流程包括这些步骤。另外，您也可以使用激光雷达相机标定器应用程序交互式地执行激光雷达-相机标定。

从相机和激光雷达传感器中提取棋盘的三维信息。要从相机数据中提取世界坐标下的三维棋盘角，请使用 estimateCheckerboardCorners3d 函数。要从激光雷达点云数据中提取棋盘平面，请使用 detectRectangularPlanePoints 函数。
您可以使用 estimateLidarCameraTransform 函数估算刚性变换矩阵。该函数以 rigidtform3d 对象的形式返回变换。

您可以使用转换矩阵来

通过计算误差来评估标定的准确性。您可以使用 estimateLidarCameraTransform 进行编程，也可以使用 Lidar Camera Calibrator 应用程序进行交互。
使用 projectLidarPointsOnImage 函数将激光雷达点投射到图像上，如图所示。
使用 fuseCameraToLidar 函数融合激光雷达和相机输出。
根据相应图像中的二维边界框估算点云中的三维边界框。更多信息，请参阅使用图像标签检测激光雷达中的车辆。

二、激光雷达和相机标定

本例向您展示如何估计三维激光雷达传感器和相机之间的刚性变换，然后使用刚性变换矩阵融合激光雷达和相机数据。

2.1 概述

激光雷达传感器和相机通常在自动驾驶应用中结合使用，因为激光雷达传感器收集三维空间信息，而相机则以二维图像捕捉空间的外观和纹理。您可以融合来自这些传感器的数据来改进物体检测和分类。激光雷达-相机标定可以估算出一个变换矩阵，给出两个传感器之间的相对旋转和平移。在进行激光雷达-相机数据融合时，您可以使用该矩阵。

本图说明了激光雷达和相机标定 (LCC) 过程的工作流程，我们使用棋盘格作为标定对象。我们从激光雷达和相机数据中提取棋盘角和平面，然后在它们的坐标系之间建立几何关系，进行标定。有关激光雷达-相机标定过程的更多信息，请参阅什么是激光雷达-相机标定？

本示例使用了两个不同激光雷达传感器的数据，一个是 VelodyneLiDAR® HDL-64 传感器，另一个是 VelodyneLiDAR® VLP-16 传感器。对于 HDL-64 传感器，使用从 Gazebo 环境中采集的数据。

HDL-64 传感器捕获的数据是一组 PNG 图像和相应的 PCD 点云。本示例假定您已经知道相机的固有参数。有关提取相机内参标定参数的详细信息，请参阅评估单相机标定的准确性。

2.2 加载数据

从 Gazebo 加载 Velodyne HDL-64 传感器数据。

imagePath = fullfile(toolboxdir('lidar'),'lidardata','lcc','HDL64','images');
ptCloudPath = fullfile(toolboxdir('lidar'),'lidardata','lcc','HDL64','pointCloud');
cameraParamsPath = fullfile(imagePath,'calibration.mat');

% Load camera intrinsics.
intrinsic = load(cameraParamsPath);

% Load images using imageDatastore.
imds = imageDatastore(imagePath);
imageFileNames = imds.Files;

% Load point cloud files.
pcds = fileDatastore(ptCloudPath,'ReadFcn',@pcread);
ptCloudFileNames = pcds.Files;

% Square size of the checkerboard.
squareSize = 200;

% Set random seed to generate reproducible results.
rng('default')

2.3 检测棋盘角

本例使用棋盘格图案进行标定。首先，根据相机数据估算棋盘边缘。使用 estimateCheckerboardCorners3d 函数计算棋盘角的坐标和实际棋盘的尺寸（以毫米为单位）。该函数以世界坐标系中的三维坐标来估算边角。

[imageCorners3d,checkerboardDimension,dataUsed] = ...
    estimateCheckerboardCorners3d(imageFileNames,intrinsic.cameraParams,squareSize);

% Remove image files that are not used.
imageFileNames = imageFileNames(dataUsed);

使用 helperShowImageCorners 辅助函数将结果可视化。

% Display checkerboard corners.
helperShowImageCorners(imageCorners3d,imageFileNames,intrinsic.cameraParams)

2.4 检测棋盘平面

接下来，使用 detectRectangularPlanePoints 函数检测激光雷达数据中的棋盘平面。该函数使用 estimateCheckerboardCorners3d 函数计算出的棋盘尺寸来检测棋盘平面。

% Extract the checkerboard ROI from the detected checkerboard image corners.
roi = helperComputeROI(imageCorners3d,5);

% Filter the point cloud files that are not used for detection.
ptCloudFileNames = ptCloudFileNames(dataUsed);
[lidarCheckerboardPlanes,framesUsed,indices] = ...
    detectRectangularPlanePoints(ptCloudFileNames,checkerboardDimension,ROI=roi);

% Remove ptCloud files that are not used.
ptCloudFileNames = ptCloudFileNames(framesUsed);

% Remove image files.
imageFileNames = imageFileNames(framesUsed);

% Remove 3-D corners from images.
imageCorners3d = imageCorners3d(:,:,framesUsed);

使用 helperShowCheckerboardPlanes 函数将检测到的棋盘可视化。

helperShowCheckerboardPlanes(ptCloudFileNames,indices)

2.5 标定激光雷达和相机

使用 estimateLidarCameraTransform 函数估算激光雷达传感器和相机之间的刚性变换矩阵。

[tform,errors] = estimateLidarCameraTransform(lidarCheckerboardPlanes, ...
    imageCorners3d,intrinsic.cameraParams);

标定后，您可以使用此变换矩阵来

使用 projectLidarPointsOnImage 函数在图像上投射激光雷达点云。
使用 fuseCameraToLidar 函数，利用图像中的颜色信息增强激光雷达点云。

使用 helperFuseLidarCamera 函数将激光雷达和图像数据融合在一起，实现可视化。

helperFuseLidarCamera(imageFileNames,ptCloudFileNames,indices, ...
    intrinsic.cameraParams,tform);

2.6 标定误差可视化

您可以使用这些类型的误差来估算标定精度。

平移误差 - 点云中棋盘格平面的中心点坐标与相应图像中的中心点坐标之间的差值（单位：米）。
旋转误差 - 点云中棋盘格平面所定义的法线角度与相应图像中的法线角度之差，单位为度。
重投影误差 - 从点云中投影（转换）的棋盘格平面的中心点坐标与相应图像中的中心点坐标之间的差值，单位为像素。

使用 helperShowError 函数绘制估计误差值。

helperShowError(errors)

2.7 在真实数据上标定

在实际 VLP-16 激光雷达数据上测试 LCC 工作流程，以评估其性能。

clear
imagePath = fullfile(toolboxdir('lidar'),'lidardata','lcc','vlp16','images');
ptCloudPath = fullfile(toolboxdir('lidar'),'lidardata','lcc','vlp16','pointCloud');
cameraParamsPath = fullfile(imagePath,'calibration.mat');

% Load camera intrinscs.
intrinsic = load(cameraParamsPath);                  

% Load images using imageDatastore.
imds = imageDatastore(imagePath);                    
imageFileNames = imds.Files;

% Load point cloud files.
pcds = fileDatastore(ptCloudPath,'ReadFcn',@pcread); 
ptCloudFileNames = pcds.Files;

% Square size of the checkerboard in mm.
squareSize = 81;                                     

% Set random seed to generate reproducible results.
rng('default')

% Extract checkerboard corners from the images.
[imageCorners3d,checkerboardDimension,dataUsed] = ...
    estimateCheckerboardCorners3d(imageFileNames,intrinsic.cameraParams,squareSize);

% Remove the unused image files.
imageFileNames = imageFileNames(dataUsed);           

% Filter the point cloud files that are not used for detection.
ptCloudFileNames = ptCloudFileNames(dataUsed);

% Extract ROI from the detected checkerboard image corners.
roi = helperComputeROI(imageCorners3d,5);

% Extract checkerboard plane from point cloud data.
[lidarCheckerboardPlanes,framesUsed,indices] = detectRectangularPlanePoints( ...
    ptCloudFileNames,checkerboardDimension,RemoveGround=true,ROI=roi);
imageCorners3d = imageCorners3d(:,:,framesUsed);

% Remove ptCloud files that are not used.
ptCloudFileNames = ptCloudFileNames(framesUsed);

% Remove image files that are not used.
imageFileNames = imageFileNames(framesUsed);

[tform,errors] = estimateLidarCameraTransform(lidarCheckerboardPlanes, ...
    imageCorners3d,intrinsic.cameraParams);
helperFuseLidarCamera(imageFileNames,ptCloudFileNames,indices, ...
    intrinsic.cameraParams,tform);

% Plot the estimated error values.
helperShowError(errors);

2.8 总结

本示例概述了激光雷达-相机标定工作流程，并向您展示了如何使用刚性变换矩阵来融合激光雷达和相机数据。

2.9 参考资料

[1] Zhou, Lipu, Zimo Li, and Michael Kaess. “Automatic Extrinsic Calibration of a Camera and a 3D LiDAR Using Line and Plane Correspondences.” In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 5562–69. Madrid: IEEE, 2018. https://doi.org/10.1109/IROS.2018.8593660.

[2] Arun, K. S., T. S. Huang, and S. D. Blostein. “Least-Squares Fitting of Two 3-D Point Sets.” IEEE Transactions on Pattern Analysis and Machine Intelligence PAMI-9, no. 5 (September 1987): 698–700. https://doi.org/10.1109/TPAMI.1987.4767965.

三、开始使用激光雷达相机标定器

激光雷达相机标定程序可让您通过估算激光雷达传感器和相机之间的刚性变换，以交互方式在两者之间执行标定。

本专题将向您展示 Lidar Camera Calibrator 应用程序的工作流程，以及您可以用来分析和改进结果的功能。标定过程的第一个也是最重要的部分是获取准确有用的数据。有关获取数据的指南和技巧，请参阅标定指南。

3.1 加载数据

要打开 Lidar Camera Calibrator 应用程序，请在 MATLAB® 命令提示符下输入此命令。

lidarCameraCalibrator

或者，也可以从 "应用程序 "选项卡的 "图像处理和计算机视觉 "下打开该应用程序。

应用程序打开时是一个空会话。该程序可读取 PLY 和点云数据 (PCD) 格式的点云数据，以及 imformats 支持的任何格式的图像。如果您的数据存储在 rosbag 文件中，请参阅 "从 Rosbag 文件读取激光雷达和相机数据 "教程进行相应转换。

将标定数据加载到应用程序中。

在应用程序工具条上，选择导入 > 导入数据，打开导入数据对话框。
在图像文件夹框中，输入包含要加载的图像文件的文件夹路径。或者，选择 "浏览 "导航到包含图像的文件夹，然后单击 "选择文件夹"。
在点云文件夹框中，输入包含要加载的 PCD 或 PLY 文件序列的文件夹路径。或者，选择 "浏览 "导航到包含文件的文件夹，然后单击 "选择文件夹"。
在 "棋盘格设置 "部分，输入标定棋盘格参数。在 "方格尺寸 "框中指定每个棋盘方格的尺寸，并从框旁边的列表中选择测量单位。
在填充框中，输入棋盘的填充值。有关填充的更多信息，请参阅棋盘填充。单击 "确定 "导入数据。

输入的图像和点云文件必须具有相同的名称。应用程序会使用文件名将图像和点云数据配对。它会将图像与具有相同文件名的相应点云进行比较。

提示

要在会话中的任意位置向会话添加更多图像和点云，请选择导入 > 向会话添加数据。

3.2 检测特征

加载图像和点云数据后，应用程序会对其执行自动特征检测。应用程序会使用指定的棋盘格参数，从图像数据中检测棋盘格角，从点云数据中检测棋盘格面。

当应用程序在图像和点云中都检测到棋盘格特征时，会将其显示在 "接受数据 "窗格中，并接受图像和点云数据对进行标定。

如果应用程序在图像、点云或两者中均未检测到特征，则会将其显示在 "拒绝数据 "窗格中。您可以使用 "选择感兴趣区域 "或 "选择棋盘格区域 "工具来检测剔除数据中的特征。

从数据浏览器的 "接受数据 "或 "剔除数据 "窗格中选择数据对，将其显示在可视化窗格中。

您可以在应用程序工具条的 "操作 "部分使用这些工具来处理接受和剔除的数据对。

移动到已接受数据 - 将数据对从已拒绝数据移动到已接受数据。
移动到拒绝数据 - 将数据对从接受数据移动到拒绝数据。
删除 - 删除选定的数据对。

有关在数据浏览器中使用的键盘快捷键列表，请参阅数据浏览器。

加载相机固有参数

默认情况下，应用程序会在内部计算输入图像数据中的相机固有参数，以执行特征检测。您也可以在应用程序工具条的 "相机固有参数 "部分选择 "使用固定固有参数"，将相机固有参数加载到应用程序中。在打开的对话框中，指定相机固有参数的位置并将其加载到应用程序中。您可以从文件或 MATLAB 工作区加载固有参数。然后，在 "检测特征 "部分，选择 "检测 "以使用新的固有参数重新检测输入数据中的特征。

要将相机固有参数重置为应用程序计算的默认值，请选择计算固有参数。要加载不同的固有参数值，请选择使用固定固有参数，然后选择加载固有参数。

选择感兴趣的区域

指定感兴趣区域来检测特征可以改善特征检测结果，尤其是在剔除数据中。要指定感兴趣区域，首先要从应用工具条中选择编辑 ROI。这将打开 "编辑 ROI "选项卡。

在 "编辑 ROI "选项卡中，应用程序会显示带有 ROI 立方体的点云数据。调整 ROI 立方体，使其与包含棋盘格的区域更加匹配。这样可以将特征检测限制在特定区域内，从而减少数据剔除并提高性能。

使用以下步骤，使用 "编辑 ROI "微调点云数据中的棋盘格检测。

选择任意数据对。您可以选择一个被剔除的数据对来调整 ROI。
要调整 ROI，请在 "编辑 ROI "选项卡的点云窗格中，暂停 ROI 立方体。指针会变成一个手形符号。单击 ROI 的任意一侧并拖动，调整长方体尺寸。要锁定长方体尺寸，请右键单击长方体并选择锁定尺寸。您还可以撤销和重做对 ROI 的修改。
选择 "捕捉到 ROI "可视化 ROI 长方体内的棋盘格点，并相应调整长方体大小。要查看整个点云，请清除 "捕捉到 ROI"。
您可以更新每个单独坐标系的 ROI，也可以一次性更新所有坐标系的 ROI。
选择 "应用 "保存更改，或选择 "取消 "放弃更改。

更新点云帧的 ROI 后，在应用程序工具条的 "标定 "选项卡上的 "检测特征 "部分，选择 "检测 "以重新检测所选 ROI 内输入数据中的特征。

提示

使用键盘快捷键可以更加交互式地执行这些任务。有关 "编辑 ROI "键盘快捷键，请参阅 "编辑 ROI"。

选择棋盘格区域

要进一步调整特征检测，请单击应用程序工具条上的 "选择棋盘区域"。应用程序会打开 "选择棋盘 "选项卡，您可以在其中手动选择任意点云坐标系中的棋盘点。

在 "选择棋盘 "选项卡中

选择任意数据对。您可以选择一个被剔除的数据对，然后在点云中找到棋盘格。
使用点云显示轴工具栏中的缩放和旋转选项来定位棋盘格。
单击 "选择棋盘格"，光标将变为十字准线。
在棋盘上单击并拖动光标。这时会出现一个矩形选区，选区内的点以红色高亮显示。或者，也可以选择坐标轴工具栏上的刷子/选择数据。
选择点后，旋转点云，检查是否有背景点被选中。如果您的选择包含不需要的点，请选择 "擦除 "重新开始。
选择 "应用 "保存选中的点，或选择 "取消 "放弃选中的点。

棋盘格选择只适用于所选点云。更新棋盘格点后，选择 "检测 "可使用更新后的棋盘格点重新检测输入数据中的地物。

特征检测设置

应用程序提供了这些特征检测设置，您可以利用它们调整检测参数。

移除地面 - 从点云中移除地面点。该应用程序使用 pcfitplane 函数估算地平面。移除地平面功能默认已启用。选择 "移除地面 "可将其清除。
聚类阈值 - 以米为单位指定点云中两个相邻点的聚类阈值。聚类过程基于相邻点之间的欧氏距离。如果相邻两点之间的距离小于聚类阈值，则两点属于同一聚类。低分辨率激光雷达传感器需要较高的聚类阈值，而高分辨率激光雷达传感器则需要较低的聚类阈值。
维度容差 - 矩形平面维度的不确定性容差，指定范围为 [0,1]。尺寸公差越大，表示矩形平面尺寸的公差范围越大。

更新新的检测参数后，选择 "检测 "以重新检测输入数据中的特征。

3.3 标定

对检测结果满意后，选择标定按钮对传感器进行标定。如果您有估计的变换矩阵，请选择初始变换从文件或工作区加载变换矩阵。本应用程序假定激光雷达传感器和相机之间的旋转角度在 [-45 45 ]范围内，单位为度，沿每个轴。如果旋转角度超出此范围，请使用初始变换指定初始变换以提高标定精度。

标定完成后，应用程序界面会显示图像，并将点云中的棋盘格点投射到图像上。应用程序使用 projectLidarPointsOnImage 函数将激光雷达点投射到图像上。使用 fuseCameraToLidar 函数将图像的颜色信息与点云数据融合。

该程序还利用误差图提供转换矩阵的不准确度指标。图中说明了每个数据对中的这些误差：

平移误差 - 点云中棋盘格平面的中心点坐标与相应图像中的中心点坐标之差。应用程序以米为单位返回误差值。
旋转误差 - 点云中棋盘格平面所定义的法线角度与相应图像中的法线角度之间的差值。应用程序使用棋盘格角坐标估算图像中的平面。应用程序会以度数为单位返回误差值。
重投影误差-- 点云中棋盘格平面的投影（变换）中心点坐标与相应图像中棋盘格平面的投影（变换）中心点坐标之差。应用程序以像素为单位返回误差值。

在数据浏览器中选择一个数据对时，误差图中的相应条形图将以深蓝色突出显示。您可以通过删除异常值来调整标定结果。垂直拖动每个图上的红线可设置误差限值。应用程序会选择误差值大于误差限值的所有数据对作为异常值，并在数据浏览器中以蓝色高亮显示误差条及其对应的数据对。右键单击数据浏览器中任何选定的数据对，然后选择删除和重新校准，即可删除异常值并重新校准传感器。删除异常值可以提高标定精度。有关误差图的键盘快捷键列表，请参阅误差图。

3.4 导出结果

您可以将转换矩阵和误差指标作为变量导出到工作区或 MAT 文件中。您可以生成完整应用工作流程的 MATLAB 脚本，以便在项目中使用。

3.5 局限性

激光雷达相机标定程序有以下限制：

从 "导出">"生成 MATLAB 脚本 "生成的脚本不包括使用 "选择棋盘格 "功能手动选择的任何棋盘格区域。在脚本中，棋盘格区域是在指定的 ROI 中检测到的。
使用 "选择棋盘格 "功能手动选择棋盘格区域后，返回 "标定 "选项卡时，只能在查看整个点云时（清除 SnapToROI 时）看到选定的点（红色突出显示）。

四、从 Rosbag 文件读取激光雷达和相机数据

本例演示了如何从 rosbag 文件中读取并保存图像和点云数据。本例还展示了如何为激光雷达相机标定准备数据。

使用本示例末尾定义的 helperDownloadRosbag 辅助函数下载 rosbag 文件。

path = helperDownloadRosbag;

从 bag 文件中读取信息。

bag = rosbag(path);

从 rosbag 中选择图像和点云消息，并通过使用相应的话题名称从文件中选择消息子集。您还可以使用时间戳过滤数据。更多信息，请参阅选择（ROS 工具箱）功能。

imageBag = select(bag,'Topic','/camera/image/compressed');
pcBag = select(bag,'Topic','/points');

阅读所有消息。

imageMsgs = readMessages(imageBag);
pcMsgs = readMessages(pcBag);

要为激光雷达相机标定准备数据，必须对两个传感器的数据进行时间同步。为选定的话题创建时间序列（ROS 工具箱）对象并提取时间戳。

ts1 = timeseries(imageBag);
ts2 = timeseries(pcBag);
t1 = ts1.Time;
t2 = ts2.Time;

要进行准确的标定，必须以相同的时间戳采集图像和点云。根据时间戳匹配两个传感器的相应数据。为考虑不确定性，使用 0.1 秒的余量。

k = 1;
if size(t2,1) > size(t1,1)
    for i = 1:size(t1,1)
        [val,indx] = min(abs(t1(i) - t2));
        if val <= 0.1
            idx(k,:) = [i indx];
            k = k + 1;
        end
    end
else
    for i = 1:size(t2,1)
        [val,indx] = min(abs(t2(i) - t1));
        if val <= 0.1
            idx(k,:) = [indx i];
            k = k + 1;
        end
    end
end

创建保存有效图像和点云的目录。

pcFilesPath = fullfile(tempdir,'PointClouds');
imageFilesPath = fullfile(tempdir,'Images');
if ~exist(imageFilesPath,'dir')
    mkdir(imageFilesPath);
end
if ~exist(pcFilesPath,'dir')
    mkdir(pcFilesPath);
end

提取图像和点云。将文件命名并保存在各自的文件夹中。将相应的图像和点云保存在同一编号下。

for i = 1:length(idx)
    I = readImage(imageMsgs{idx(i,1)});
    pc = pointCloud(readXYZ(pcMsgs{idx(i,2)}));
    n_strPadded = sprintf('%04d',i) ;
    pcFileName = strcat(pcFilesPath,'/',n_strPadded,'.pcd');
    imageFileName = strcat(imageFilesPath,'/',n_strPadded,'.png');
    imwrite(I,imageFileName);
    pcwrite(pc,pcFileName);
end

启动激光雷达相机标定器应用程序，使用界面将数据加载到应用程序中。您也可以从 MATLAB® 命令行加载数据并启动应用程序。

checkerSize = 81; %毫米
padding = [0 0 0 0]；
lidarCameraCalibrator(imageFilesPath,pcFilesPath,checkerSize,padding)

4.1 支持功能

function rosbagFile = helperDownloadRosbag()
% Download the data set from the given URL.
rosbagZipFile = matlab.internal.examples.downloadSupportFile( ...
    'lidar','data/lccSample.zip');
[outputFolder,~,~] = fileparts(rosbagZipFile);
rosbagFile = fullfile(outputFolder,'lccSample.bag');
if ~exist(rosbagFile,'file')
    unzip(rosbagZipFile,outputFolder);
end
end