相机-激光雷达标定 - V 2.0

这是ITSC 2021论文"优化样本选择以实现稳健的激光雷达相机标定"官方代码发布的第二个版本。

注意：要访问此代码的1.0版本，即上述论文的原始实现，请切换到标记为master的分支。

该软件包估计将相机坐标系(父坐标系)转换为激光雷达坐标系(子坐标系)的标定参数。我们旨在通过优化姿态选择过程来简化标定过程，避免繁琐的反复尝试不同姿态直到找到良好标定结果的试错过程。我们寻求获得适合整个场景而不仅仅是目标的带不确定性的标定参数估计，这是许多现有工作难以做到的。我们提出的方法克服了现有基于目标的标定方法的局限性，即用户误差和对目标的过拟合。更多详细信息，请参阅我们的论文。

<img width="70%" src="https://yellow-cdn.veclightyear.com/835a84d5/f5c7dac7-29f3-4513-8bef-c6400bc5abb6.png"> 左图: 我们在澳大利亚野外机器人中心(ACFR)的传感器设置。右图: 该软件包对Nvidia gmsl相机与Baraja Spectrum-Scan™(上)和Velodyne VLP-16(下)的标定结果。Baraja Spectrum-Scan™的投影在棋盘上有一些地面点(黄色)，这是由于相机和激光雷达视角的差异造成的。

注意: 在论文中，显示条件数方程的公式(2)有一个错误。本仓库中实现的正确计算条件数的公式是： conditionnum_formula

更新日志 - V2有哪些变化？

使用特征提取用户界面创建一个静态场景的感兴趣区域
对静态场景进行采样
将捕获的样本从每个连续帧中减去，以提取场景中的任何其他新物体。这用于区分帧中的棋盘
捕获样本 - 现在获取5个连续帧并得到棋盘参数的运行平均值
根据需要获取尽可能多的样本
优化功能与以前相同

其他变化

可视化结果脚本处理角度包裹，以停止在+/-2Pi处突然的角度符号波动
设置了pre-commit
强制使用Clang格式化所有文件以保持一致性，之前很难阅读且非常需要一致性。

1. 入门

1.1 安装

该软件包仅在ROS Melodic中进行过测试。

本地ROS

在您的catkin_ws/src/文件夹中克隆仓库

git clone -c http.sslverify=false -b calib-v2 https://github.com/acfr/cam_lidar_calibration

下载ros和python依赖

sudo apt update && sudo apt-get install -y ros-melodic-pcl-conversions ros-melodic-pcl-ros ros-melodic-tf2-sensor-msgs
pip install pandas scipy

构建软件包并source setup.bash或setup.zsh文件。

catkin build cam_lidar_calibration
source ~/catkin_ws/devel/setup.bash

Docker

在您的catkin_ws/src/文件夹中克隆仓库

git clone -c http.sslverify=false -b calib-v2 https://github.com/acfr/cam_lidar_calibration

运行docker镜像(将从ockerhub拉取)。如果您的计算机有Nvidia GPU，设置cuda标志为--cuda on。如果没有，设置为--cuda off。

cd cam_lidar_calibration/docker
./run.sh --cuda on

运行此脚本后，docker容器将运行并立即构建catkin工作空间并source setup.bash文件。完成后，您可以继续快速入门部分。

如果您想从头构建镜像，我们也提供了build.sh脚本。

注意：如果使用docker，./run.sh会将您本地的cam_lidar_calibration文件夹挂载到容器内的/catkin_ws/src/cam_lidar_calibration。运行标定时，这会在容器内以root所有权创建csv文件，这并不理想。但是解决方法是在docker镜像外使用以下命令，这会将当前文件夹中的所有文件的所有权更改为与本地环境中的$USER和$GROUP相同。

sudo chown $USER:$GROUP *

1.2 快速入门

您可以通过在我们提供的快速入门数据上运行此软件包来验证该仓库是否成功运行。如果您使用docker，这些指令应在容器内运行。

1. 运行标定过程

第一步使用保存的姿态，计算具有最低VOQ分数的最佳集合。

roslaunch cam_lidar_calibration run_optimiser.launch import_samples:=true

标定后，输出保存在与导入样本相同的目录中。对于这个快速入门示例，输出保存在cam_lidar_calibration/data/vlp/中。

2. 获取并评估标定结果

这一步通过对最佳集合进行过滤平均来给出估计的标定参数，并显示估计参数的高斯拟合直方图。此外，我们通过计算所有提供的数据样本的重投影误差和可视化(如果指定)来提供标定结果的评估。

要获取并评估标定输出，请提供第一步生成的csv输出文件的绝对路径。以下示例使用预先计算的标定结果。如果您愿意，可以用第1步中新生成的结果替换它。您应该看到带有重投影误差的终端输出，以及高斯拟合直方图和可视化结果。

roslaunch cam_lidar_calibration assess_results.launch csv:="$(rospack find cam_lidar_calibration)/data/vlp/calibration_quickstart.csv" visualise:=true

就是这样!如果这个快速入门成功运行,您就可以开始将这个工具用于自己的数据了。如果没有成功,请创建一个问题,我们会尽快解决。

2. 使用您自己的数据进行标定

要将此软件包用于您自己的数据,请确保您的bag文件包含以下话题:

激光雷达: 点类型为XYZIR的3D点云,以sensor_msgs::PointCloud2格式发布。该软件包依赖于ring值,如果您没有这个值,则需要修改激光雷达驱动程序以使用此软件包。
单目相机: 以sensor_msgs::Image格式发布的图像和相应的元信息话题(sensor_msgs::CameraInfo)。

所有数据和输出文件将保存在cam_lidar_calibration/data/YYYY-MM-DD_HH-MM-SS/文件夹中。

2.1 标定目标的设置

准备一个矩形棋盘格打印件。论文中使用的棋盘格打印件是A1尺寸(594 x 841mm),方格大小为95mm,内部顶点为7x5(不同于网格方格的数量),可从https://markhedleyjones.com/projects/calibration-checkerboard-collection 下载
将棋盘格牢固地贴在一个刚性、不透明的矩形板上,使两者的中心尽可能对齐,边缘保持平行。
选择一个合适的支架,可以安装目标,尽量不要有突出板边缘的元素。
旋转棋盘格,使其呈菱形状(相对于地面成45°角),并将其安装在支架上。

在下图中,我们展示了两个与此软件包一起使用的棋盘格装置。

<img width="30%" src="https://yellow-cdn.veclightyear.com/835a84d5/ec9ebcb1-2dab-4547-8ac3-046133cb7d62.png"> 左图: 内部顶点为8x6,方格大小为65mm的棋盘格。右图: 内部顶点为7x5,方格大小为95mm的棋盘格。

2.2 配置文件

以下解释了/cfg/params.yaml中的字段

1. 指定激光雷达和相机话题的名称。 例如,在我们的情况下是:

camera_topic: "/gmsl/A0/image_color"
camera_info: "/gmsl/A0/camera_info"
lidar_topic: "/velodyne/front/points"

2. (可选)指定点云过滤的默认边界。如果您不确定,可以跳过这一步。

3. 输入您准备的棋盘格目标的详细信息:

pattern_size: 这是棋盘格的内部顶点(不是方格的数量;参见第2.1节中我们的棋盘格)
square_length (mm): 棋盘格方格的长度。
board_dimension (mm): 棋盘格打印件所安装的背板的宽度和高度。
translation_error: 棋盘格中心相对于背板中心的偏移(见下图)。

<img width="40%" src="https://yellow-cdn.veclightyear.com/835a84d5/38965363-d36a-496c-91fd-5e43515c0d53.png"> 示例: 在这个例子中,偏移量为x=10mm,y=30mm,板尺寸为910x650mm,方格长度为65mm,图案大小为8x6(高x宽)。

2.3 捕获姿态并获取最佳标定参数集

1. 启动标定程序

运行标定程序,将import_samples标志设置为false。应该会打开一个rviz窗口和rqt动态重新配置窗口。如果您使用的是docker容器且RViz没有打开,请尝试将cuda标志设置为与您使用的相反的值。

roslaunch cam_lidar_calibration run_optimiser.launch import_samples:=false

这个过程可以在线或离线进行。如果您是离线操作,请确保使用--pause标志播放rosbag,以便rviz可以获取/tf话题,例如rosbag play --pause mybag.bag。如果您正在运行docker容器,可以在docker容器外的单独终端窗口中运行bag文件。

确保将图像话题更改为您的相机图像话题,以便查看视频源。这可以通过编辑cam_lidar_calibration.rviz的第62行来完成,或者打开rviz->panels->display,并更改当前为/gmsl/A0/image_color的字段。

2. 扫描不包含棋盘格的静态场景

使用rqt_reconfigure窗口,修改x、y和z轴限制的值,使其只显示场景中没有移动主体的区域。例如,我们在一个大型实验室工作,那里人员常常移动。使用rqt_reconfigure,我们能够在点云中隔离出感兴趣的区域。当您对静态场景满意时,按下"Capture Background"。一旦捕获了背景场景,这个软件包将在每一帧中执行背景减除,以自动隔离场景中的新物体。

3. 棋盘格的第一个样本

将棋盘格垂直于地面放置,位于中间,面向激光雷达和相机。我们建议第一个样本是板子的一个非常清晰的拍摄,因此建议将其直立放置并大致位于中间。

按下"Capture Sample"按钮。确保除了板子及其三脚架外,画面中没有其他东西。板子提取功能在从三脚架中检测板子方面做得很好。

确保棋盘格被正确地勾勒出轮廓,并且板子尺寸误差较小。如果不是,那么"Discard Sample"并再次点击"Capture Sample"(或移动板子并再次捕获)。 主板错误（终端窗口中）：尽量使主板尺寸误差接近零（小于30mm的误差是可以接受的）。如果主板尺寸误差太大，请在不同位置再次尝试或参考下面的潜在解决方法。

主板误差过高可能是由于棋盘太靠近或离激光雷达太远。因此我们建议将其稍微移近或移远一些。
低分辨率的激光雷达可能难以以较低误差捕捉到棋盘，如果棋盘上的点不够多的话。例如，对于VLP-16，我们至少需要棋盘上有7个环才能进行decent捕捉。
如果棋盘始终被低估或高估，且误差量相同，那么可能是激光雷达的内部距离估计没有正确校准。激光雷达在不同距离范围内通常有约±30mm的范围误差，且这个误差并不一致。我们在run_optimiser.launch文件中提供了一个参数，允许你对这个距离估计应用偏移。尝试设置偏移值，使你的数据平均误差最低（你可能需要重新捕捉几次来确定这个值）。对于我们的VLP-16，我们必须设置distance_offset_mm=-30。一旦完成校准，这个值应该在激光雷达驱动中永久设置。

<img width="50%" src="https://yellow-cdn.veclightyear.com/835a84d5/c061552b-2e94-4c82-883e-ffaa7b3c7c7d.png"> 在上图左侧，我们展示了同一姿态在3种不同距离偏移下的情况。我们通过将笛卡尔坐标(xyz)转换为极坐标(r, theta)，并在半径r上添加距离偏移来实现距离偏移。当我们这样做时，你可以想象成是扩大/缩小圆的半径。因此，新坐标系中的每个形状都被放大/缩小。增加距离偏移值会增加棋盘面积（+100mm是最大的）。右图与左图相同，只是从不同角度展示，以显示棋盘高度和宽度都有增加。

4. 棋盘的后续样本

对于每次其他捕捉，将棋盘放在不同的配置和位置，并根据需要获取尽可能多的样本。在捕捉样本之前，确保你或移动棋盘的人在放置棋盘到新姿态后不在场景中。

如果主板尺寸误差不低，你可以随时通过按"丢弃样本"按钮来丢弃最后捕捉的样本。

姿态的数量和变化：我们建议你至少捕捉10-20个姿态，最近和最远姿态之间至少有1-2m的距离范围（从激光雷达中心测量）。以下列出一些指导原则。

将姿态分散在校准范围内，覆盖图像视野的宽度。对于我们特定的VLP-16和Baraja Spectrum Scan激光雷达，范围分别为1.7m - 4m和2.1m - 5m。
尽可能让棋盘的偏航和俯仰角有变化。这在下图中有解释。

<img width="70%" src="https://yellow-cdn.veclightyear.com/835a84d5/6fc1e80f-d9ad-42b4-9b6e-eb1af89223e0.png"> 对于不好的姿态（左），棋盘的法线对齐使得它们的尖端画出一条线，并且它们都在同一位置，从而增加了在该位置过拟合棋盘的机会。对于好的姿态（右），我们看到棋盘的方向和位置有变化。

5. 优化

一旦你对你的样本满意，按下"优化"按钮。

注意，如果你不点击此按钮，姿态将不会被正确保存。

姿态被保存（png、pcd、poses.csv）在($cam_lidar_calibration)/data/YYYY-MM-DD_HH-MM-SS/文件夹中，用于重投影评估阶段（如果你希望用相同的数据重新校准也可以使用）。优化过程将在同一文件夹中生成输出文件calibration_YYYY-MM-DD_HH-MM-SS.csv，该文件存储了最佳集合的结果。

2.4 估计参数和评估重投影误差

获得校准csv输出文件后，将校准输出文件的绝对路径复制粘贴到下面命令中csv:=后面，用双引号括起来。应该会出现一个带有高斯拟合的直方图。如果你设置了可视化标志，你可以选择可视化一个样本。如果你想可视化不同的样本，可以在assess_results.launch文件中更改特定样本。重投影结果显示在终端窗口中。

最终估计的校准参数可以在终端窗口中找到或从直方图中获取。

roslaunch cam_lidar_calibration assess_results.launch csv:="$(rospack find cam_lidar_calibration)/data/vlp/calibration_quickstart.csv" visualise:=true

<img width="70%" src="https://yellow-cdn.veclightyear.com/835a84d5/dbe91712-592d-49f6-b88d-a72ebae7dddf.png"> 我们校准管道的输出显示了一个带有高斯拟合的直方图和校准结果的可视化，包括重投影误差。