R3LIVE项目实战(4) — 双目相机与激光雷达livox_camera_lidar_calibration联合标定

本方案提供了一个手动校准Livox雷达和相机之间外参的方法，已经在LIVOX AVIA和ZED2i上进行了验证。其中包含了计算相机内参，获得标定数据，优化计算外参和雷达相机融合应用相关的代码。本方案中使用了标定板角点作为标定目标物，由于Livox雷达非重复性扫描的特点，点云的密度较大，比较易于找到雷达点云中角点的准确位置。相机雷达的标定和融合也可以得到不错的结果。

步骤1: 环境配置

(以下标定基于Ubuntu 20.04环境)

1.1 安装环境和驱动

安装ROS环境，安装 Livox SDK 和 livox_ros_driver 如已安装可以跳过此步骤。Livox雷达环境的配置可以参考文章：R3LIVE项目实战(2) — LIVOX AVIA激光雷达配置_livox 激光雷达_几度春风里的博客-CSDN博客

1.2 安装依赖

如已安装可以跳过此步骤。

1.3 下载源码，编译准备

git clone https://github.com/Livox-SDK/livox_camera_lidar_calibration.git
cd camera_lidar_calibration
catkin_make
source devel/setup.bash

1.4 程序节点概括

此项目包括如下节点:

cameraCalib - 标定相机内参
pcdTransfer - 将雷达点云rosbag转换成PCD文件
cornerPhoto - 获得照片角点
getExt1 - 计算外参节点１，只优化外参
getExt2 - 计算外参节点２，同时优化内参和外参
projectCloud - 把雷达点云投影到照片上
colorLidar - 雷达点云着色

以下程序节点中如果想修改launch文件，需要到src/calibration/launch文件夹中找对应的launch文件。

步骤2: 相机内参标定

2.1 前期准备

准备一块有黑白棋盘格的标定板(如下所示，可以打印出来)

2.3 cameraCalib标定

方法一：参考官方的MATLAB标定方法

方法二：使用自带的cameraCalib.launch

获得标定板各个角度的照片数据后，配置cameraCalib.launch中对应的路径和参数，默认是把照片数据放在data/camera/photos下，然后在data/camera/in.txt中写入所有需要使用的照片名称，如下图所示：

输入指令开始标定

roslaunch camera_lidar_calibration cameraCalib.launch

标定结果中会保存在data/camera/result.txt中，包括重投影误差，内参矩阵和畸变纠正参数。

方法三：使用相机官方的标定程序，以ZED相机为例，安装配置文件后在/usr/local/zed/tools/ZED_Calibration下的自动标定程序，具体看相机官方是否提供。

依次转动相机和图中红色圆圈重合即可

2.4 内参结果

一个3x3的内参矩阵(IntrinsicMatrix)
５个畸变纠正参数 k1, k2, p1, p2, k3

步骤3: 标定准备和数据采集

3.1 标定场景准备

本方案使用标定板的四个角点来作为目标物 [注 2]。标定场景最好选择一个较为空旷的环境，这样方便识别标定板，并且保证雷达到标定板有３米以上。这个案例中使用了低反射率泡棉制作的标定板(1m x 1.5m)。需要选取至少10个左右不同的角度和距离来摆放标定板(参考相机内参的标定物摆放)，左右位置和不同的偏向角度最好都有采集数据 [注 3]。

不同角度位置摆放标定板（本人不是用官方建议的泡棉）

3.2 连接雷达

检查标定板角点是否在点云中，输入点云可视化的命令查看点云

roslaunch livox_ros_driver livox_lidar_rviz.launch

需要录制rosbag时输入另一个命令

roslaunch livox_ros_driver livox_lidar_msg.launch

确认保存的rosbag数据格式是customMsg，后续程序中处理rosbag是对应的“livox custom msg format”格式。

3.3 连接相机

本方案中使用ZED双目相机，检查获取照片的质量，并检查标定板角点是否在照片中。

3.4 采集照片和点云数据

拍摄照片
运行指令录制点云

每个位置保存一张照片和10s左右的rosbag即可。数据采集完成后，将照片放在data/photo文件夹下; 雷达rosbag放在data/lidar文件夹下。

具体操作可参考博客3.5节：R3LIVE项目实战(3) —双目相机与激光雷达 lidar_camera_calib联合标定_几度春风里的博客-CSDN博客

步骤4: 标定数据获取

4.1 参数设置

首先需要把步骤２得到的内参和畸变纠正参数以下图的格式保存在data/parameters/intrinsic.txt文件下

4.2 获得照片中的角点坐标

配置cornerPhoto.launch文件中的照片路径，运行

roslaunch camera_lidar_calibration cornerPhoto.launch

程序会在UI中打开对应的照片更改cornerPhoto.launch文件中的照片路径，重复上述流程，直至获得所有照片的角点坐标。源代码中会存在显示不出角点坐标的情况，具体可以参考下面的改进版本： https://blog.csdn.net/qq_38650944/category_11751658.html

4.3 获得雷达点云中的角点坐标

首先将bag文件装化为pcd文件，在pcdTransfer.launch文件中配置相应的路径。
通过pcl_viewer选取角点。

roslaunch camera_lidar_calibration pcdTransfer.launch

pcl_viewer -use_point_picking xx.pcd

在计算前检查一下雷达和相机两个标定数据中是否每行对应的是同一个角点，并检查数据量是否相同[注 7]。

5.2 外参计算getExt1节点

计算前在getExt1.launch文件中配置好外参初值 [注 8]，输入指令开始计算外参

roslaunch camera_lidar_calibration getExt1.launch

终端中可以看到每次迭代运算的cost，外参结果以齐次矩阵的格式保存到data/parameters/extrinsic.txt下。

可以根据优化后的残差和重投影误差评估一下得到的外参 [注 9]。

重投影会把误差较大的数据打印在屏幕上，可以剔除异常标定数据后再重新进行优化计算。

5.3 外参计算getExt２节点

getExt1节点只优化外参，而getExt2节点在计算的时候会将一开始计算的内参作为初值和外参一起优化。输入指令程序会得到一个新的内参和外参，并用新的参数来进行重投影验证。

roslaunch camera_lidar_calibration getExt2.launch

一般使用getExt1节点即可，如果在外参初值验证过，并且异常值已经剔除后，优化还是有较大的残差，那么可以使用getExt2试一试。使用的前提需要保证标定数据量较大，并且要充分验证结果。

如果经过验证getExt2计算的结果确实更好，那么把新的内参更新在data/parameters/intrinsic.txt中。

注意：在R3LIVE中设置外参时，要对此处的得到的外参结果求逆，不然得不到最后的点云着色效果。具体操作是将R按对角线反转，T取负号。

步骤６: 结果验证与相关应用

6.1 概述

获得外参后我们可以用两个常见的应用看一下融合的效果。第一个是将点云投影到照片上，第二个是点云的着色[注 10]。

6.2 投影点云到照片

在projectCloud.launch文件中配置点云和照片的路径后，运行指令，将rosbag中一定数量的点投影到照片上并且保存成新的照片。

roslaunch camera_lidar_calibration projectCloud.launch

6.3 点云着色

在colorLidar.launch文件中配置点云和照片的路径，运行指令，可以在rviz中检查着色的效果。

roslaunch camera_lidar_calibration colorLidar.launch

注解：

检查pcdTransfer.launch文件中的rosbag路径，设置rosbag的数量，并将rosbag以0.bag, 1.bag...命名。
运行指令将rosbag批量转化成PCD文件，PCD文件默认保存在data/pcdFiles文件夹中
使用pcl_viewer打开PCD文件，按住shift+左键点击即可获得对应的点坐标。注意和照片采用相同的角点顺序[注 6]。
将xyz角点坐标按如下格式保存在data/corner_lidar.txt中，将所有PCD文件中雷达点云的角点坐标保存下来。
打开pcl_viewer后可以输入"h"来获得指引。
注意少于10个字段不能被读取为计算数据，如果点云xyz或者照片xy坐标比较短需要补足10个字段。
程序中的默认初值是根据Livox激光雷达自身坐标系，雷达和相机的相对位置设置的，要根据情况进行修改。如果初值差的很大可能会导致不好的优化结果。
如果迭代结束cost还是比较大的量级(比如10的4次方), 那可能是程序中初值设置的有问题，得到的只是一个局部最优解，那么需要重新设置初值计算。
点云投影到照片是通过内外参矩阵将雷达的点云投影到照片对应的位置，点云的颜色会根据距离从近到远显示从蓝到红；点云的着色是通过点云的xyz和得到的内外参矩阵算出对应的相机像素坐标，获取到这个像素的RGB信息后再赋给点云显示出来，这样雷达点云可以显示真实的颜色。