livox_camera_lidar_calibration 功能包介绍

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

功能包名称：livox_camera_lidar_calibration

功能包使用环境： Ubuntu 64-bit 16.04

在这里插入图片描述

使用步骤

采集数据

连接雷达检查标定板角点是否在点云中

输入点云可视化的命令查看点云

roslaunch livox_ros_driver livox_lidar_rviz.launch

这样就在rviz中显示了点云

连接相机检查标定板角点是否在照片中

打开相机，检查获取照片的质量，并检查标定板角点是否在照片中。
具体方法根据相机型号来了。

采集照片和点云数据

  • 拍摄照片
  • 运行指令录制点云 
    roslaunch livox_ros_driver livox_lidar_msg.launch
rosbag record /livox/lidar
  • 每个位置保存一张照片和10s左右的rosbag即可
  • 数据采集完成后，将照片放在data/photo文件夹下; 雷达rosbag放在data/lidar文件夹下

以上步骤数据就采集好了，下面进行标定工作

标定角点

设置相机内参

首先需要把得到的内参和畸变纠正参数以下图的格式保存在data/parameters/intrinsic.txt文件下 。distortion下面对应５个畸变纠正参数，按顺序是k1和k2 (RadialDistortion)，p1和p2 (TangentialDistortion)，最后一个是k3，一般默认是０
在这里插入图片描述

获得照片中的角点坐标

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

roslaunch camera_lidar_calibration cornerPhoto.launch

内容如下:

<?xml version="1.0" encoding="UTF-8"?>
<launch>

    <param name="intrinsic_path"    value="$(find camera_lidar_calibration)/../../data/parameters/intrinsic.txt" />  <!-- intrinsic file -->
    <param name="input_photo_path"  value="$(find camera_lidar_calibration)/../../data/photo/0.bmp" />  <!-- photo to find the corner -->
    <param name="ouput_path"        value="$(find camera_lidar_calibration)/../../data/corner_photo.txt" />  <!-- file to save the photo corner -->

    <node pkg="camera_lidar_calibration" name="cornerPhoto" type="cornerPhoto" output="screen"></node>

</launch>

程序会在UI中打开对应的照片。在这个UI界面上只要把鼠标移到标定板的各个角上，窗口左下角就会显示对应的坐标数据。确定一个顺序，一般从左上角的角点开始，逆时针旋转按顺序记录下四个角点坐标。

记录完毕后选中显示的图片按任意键，进入坐标输入流程。把记录下的四个坐标”x y”按顺序输入，x和y中间要有空格(比如: “635 487”)，输入完成后输入”0 0”即可结束输入流程(如下图例所示)。程序会算出四个更精确的float类型坐标显示出来，并保存在data/corner_photo.txt中。然后按任意键结束整个流程。
在这里插入图片描述
更改cornerPhoto.launch文件中的照片路径，重复上述流程，直至获得所有照片的角点坐标。

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

检查pcdTransfer.launch文件中的rosbag路径，设置rosbag的数量，并将rosbag以0.bag, 1.bag…命名。
运行指令将rosbag批量转化成PCD文件，PCD文件默认保存在data/pcdFiles文件夹中

roslaunch camera_lidar_calibration pcdTransfer.launch

内容如下:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
    <param name="input_bag_path"        value="$(find camera_lidar_calibration)/../../data/lidar/" />  <!-- rosbag folder -->
    <param name="output_pcd_path"       value="$(find camera_lidar_calibration)/../../data/pcdFiles/" />  <!-- path to save new pcd files -->

    <param name="threshold_lidar"       type="int" value="80" />  <!-- the limit of messages to transfer to the pcd file, 80 means maximum 80 messages of lidar -->
    <param name="data_num"              type="int" value="10" />  <!-- the number of the rosbag -->

    <node pkg="camera_lidar_calibration" name="pcdTransfer" type="pcdTransfer" output="screen"></node>

</launch>

使用pcl_viewer打开PCD文件，按住shift+左键点击即可获得对应的点坐标。注意和照片采用相同的角点顺序

pcl_viewer -use_point_picking xx.pcd

将xyz角点坐标按如下格式保存在data/corner_lidar.txt中，将所有PCD文件中雷达点云的角点坐标保存下来。

上面的步骤把相机和雷达角点获得了，也就是同名点，下面就是计算外参了

参数设置

外参计算节点会读取data/corner_photo.txt和data/corner_lidar.txt中的标定数据来计算外参，数据需要保存成特定的格式才能被外参计算节点正确读取。参考以下格式，每行数据只有超过10个字母程序才会将其读取为计算的参数，比如下图用来编号的１２３４，lidar0和0.bmp这些标题不会被读取为计算参数。程序读到空行就会停止读取参数开始计算，所以保存时不要空行。
在这里插入图片描述

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

计算外参

外参计算getExt1节点

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

roslaunch camera_lidar_calibration getExt1.launch

内容如下:

<?xml version="1.0" encoding="UTF-8"?>
<launch>

    <rosparam param="init_value">  [0.0, -1.0, 0.0, 0.0,
                                    0.0, 0.0, -1.0, 0.0,
                                    1.0, 0.0, 0.0,  0.0] </rosparam>  <!-- init value of roatation matrix(3*3 on the left) and the translation(3*1 vector on the right) -->

    <param name="intrinsic_path"    value="$(find camera_lidar_calibration)/../../data/parameters/intrinsic.txt" />  <!-- intrinsic file -->
    <param name="extrinsic_path"    value="$(find camera_lidar_calibration)/../../data/parameters/extrinsic.txt" />  <!-- extrinsic file -->
    <param name="input_lidar_path"  value="$(find camera_lidar_calibration)/../../data/corner_lidar.txt" />  <!-- get the lidar corner data -->
    <param name="input_photo_path"  value="$(find camera_lidar_calibration)/../../data/corner_photo.txt" />  <!-- get the photo corner data -->
    <param name="error_threshold"    type="int" value="12" />  <!-- the threshold of the reprojection error -->

    <node pkg="camera_lidar_calibration" name="getExt1" type="getExt1" output="screen"></node>

</launch>

终端中可以看到每次迭代运算的cost，外参结果以齐次矩阵的格式保存到data/parameters/extrinsic.txt下。
在这里插入图片描述

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

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

外参计算getExt２节点

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

roslaunch camera_lidar_calibration getExt2.launch

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

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

通过上面步骤已经得到了外参的结果，结果的优略可以通过下面的验证方法

结果验证

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

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

roslaunch camera_lidar_calibration projectCloud.launch

内容如下:

<?xml version="1.0" encoding="UTF-8"?>
<launch>

    <param name="intrinsic_path"        value="$(find camera_lidar_calibration)/../../data/parameters/intrinsic.txt" />  <!-- intrinsic file -->
    <param name="extrinsic_path"        value="$(find camera_lidar_calibration)/../../data/parameters/extrinsic.txt" />  <!-- extrinsic file -->

    <param name="input_bag_path"        value="$(find camera_lidar_calibration)/../../data/lidar/0.bag" />  <!-- rosbag file -->
    <param name="input_photo_path"      value="$(find camera_lidar_calibration)/../../data/photo/0.bmp" />  <!-- photo file -->
    <param name="output_path"           value="$(find camera_lidar_calibration)/../../data/projection/new.bmp" />  <!-- path to save new photo file -->

    <param name="threshold_lidar"       type="int" value="30000" />  <!-- the maximum points shown on the photo -->

    <node pkg="camera_lidar_calibration" name="projectCloud" type="projectCloud" output="screen"></node>

</launch>

在这里插入图片描述

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

roslaunch camera_lidar_calibration colorLidar.launch

内容如下:

<?xml version="1.0" encoding="UTF-8"?>
<launch>

    <param name="intrinsic_path"        value="$(find camera_lidar_calibration)/../../data/parameters/intrinsic.txt" />  <!-- intrinsic file -->
    <param name="extrinsic_path"        value="$(find camera_lidar_calibration)/../../data/parameters/extrinsic.txt" />  <!-- extrinsic file -->

    <param name="input_bag_path"        value="$(find camera_lidar_calibration)/../../data/lidar/0.bag" />  <!-- rosbag to give the lidar info -->
    <param name="input_photo_path"      value="$(find camera_lidar_calibration)/../../data/photo/0.bmp" />  <!-- photo to get RGB info -->

    <param name="threshold_lidar"       type="int" value="80" />  <!-- the limit of messages to transfer to the pcd file, 80 means maximum 80 messages of lidar -->

  <node pkg="camera_lidar_calibration" name="colorLidar" type="colorLidar" output="screen"></node>

  <node name="rviz" pkg="rviz" type="rviz" respawn="true" args="-d $(find camera_lidar_calibration)/launch/config.rviz"/>

</launch>

在这里插入图片描述