velo2cam_calibration——最新最准确的激光雷达Lidar和相机Camera外参标定算法实现

写在前面

因为实验需求，要实现相机和雷达之间的融合，因此需要完成相机内参标定和雷达与相机外参标定。
相机内参标定使用ros自带的包camera_calibration即可完成。
具体方法可以参考博客: SLAM之相机标定
相机外参之前使用了Autoware的工具箱，但标出来的效果有一定的误差，不太满意。
velo2cam_calibration的单目相机和雷达标定是2021年才出来的一篇文章，而且算法更为鲁棒，但网上参考资料较少，坑也不算少，因此写这篇博客记录一下，希望可以帮助到有需要的人。

参考资料

这个算法的官方地址为velo2cam_calibration，这里面有源码等资料，也是唯一的参考资料。

算法原理

还是PnP的原理，通过相机和雷达观测同一个东西，并检测出来东西在各自坐标系下的误差来完成坐标系之间相对姿态的估计。
对于这个算法，这个物体指的就是标定板上的四个圆（标定板见下面的图片），相机通过检测四个Apirltag，通过标定板尺寸参数得到圆心，雷达通过透过去的洞洞检测圆心。这样把四个圆心分别对应上就可以实现外参的标定、

准备工作

一、准备一块标定板。
这个算法的标定板和其他的不一样，这也是很麻烦的一点。需要找厂家定制，板子的样式和参数详见下图。四个角上面是四个Arpiltag，因此加工需要给厂家说明，四个角上面的正方形是不需要打穿的，画上线即可。我们选用的材料是亚克力的，但拿回来发现效果没有想象中那么好，因为板子尺寸太大，在不规则放置时会存在一定的弯曲，可以试试选用木板或者钢板等。当然亚克力的也是可以用的。

加工好板子之后，四个角上还需要贴上对应的Arpiltag，这个Arpiltag的样式是不能换的，因为程序里面相机需要检测四个样式，而且位置也必须和图中的对应。这个可以直接打印出来，Arpiltag的大小没有限制，只要把图案放在正方形的中心即可（因为是识别中心的）。
四个图案我放在下面了（我是截图处理了一下，没找到原版）
依次为左上、左下、右上、右下

二、部署代码
github上开源了代码，可以直接搜到。
如果外网速度慢的同学，可以使用gitee下载，Gitee下载地址。
下载完之后安装catkin_make的一般方法编译即可，这倒是没有遇到什么问题。

开始使用

一、内参发布
我们使用的为USB相机，使用程序启动了相机并发布图像到ROS中，其主题为/camera/image_raw，消息类型为sensor_msgs::Image，其中camrea为相机名称。
但程序除去这个原始图像外，还需要知道相机的内参，也就是说需要一个camera_info的消息，这个并且主题名称应该为/相机名称/camera_info，消息类型为sensor_msgs::CameraInfo，对应到我们自身就是/camera/camera_info。
相机内参标定完成之后便可以得到一个标定结果，可以参照这篇文章来完成：
根据相机内参向ROS发布camera_info

二、打开调试按钮
为了更方便之后工作进行，可以打开DEBUG，这样便可以发布出来相机和雷达的检测结果啦。对于调试代码非常非常非常有用！！！
具体做法是修改~/catkin_ws/velo2cam_calibration/include/velo2cam.utils.h中第30行的

#define DEBUG 0

改为

#define DEBUG 1

注意修改之后需要重新编译。

三、开始调试相机
打开一个终端，输入指令

source devel/setup.bash
roslaunch velo2cam_calibration mono_pattern.launch camera_name:=/camera image_topic:=image_raw frame_name:=camera

这一部分是相机的检测，相机会通过检测四个Apirltag，估计出四个圆心的位置。
因为打开了DEBUG，应该会自动弹出一个opencv的窗口显示检测结果，如下

可以看到很好地检测到了圆心，并且终端不断输出检测结果。

如果存在全部没有检测出来的情况，可能原因是：
代码中存在两个消息/camera/image_raw和/camera_info的同步处理，有可能没有同步成功，理论来讲，会同步最近的两个消息，但不知道什么原因没有成功（至少我们测试是这样的）。解决方法就是把时间戳改为完全一样，比如将发布时候的nescs均设置为0，相机频率10，内参频率1，最后同步出来频率就是1，经过测试这样是可以同步成功的。

四、开始调试雷达
新打开一个终端窗口，输入指令（ cloud_topic）写你的点云主题名称。

source devel/setup.bash
roslaunch velo2cam_calibration lidar_pattern.launch cloud_topic:=/rslidar_points

终端会开始输出检测结果，比如找到几个目标（四个才对）。
可以打开rviz来查看检测。
雷达检测圆洞的方法是检测断点，如果周围环境太复杂会影响检测结果，因为可以人为过滤一下。
再新开一个终端，运行

rosrun rqt_reconfigure rqt_reconfigure

会打开一个过滤器，过滤掉周围的点云，在/lidar_pattern_/zyx_filtered中只留下板子和透过圆洞打过去的点，在lidar_pattern_/range_filtered_cloud中只留下板子，这样是检测效果最好的。如下

过滤完之后，可以把检测结果在RVIZ中显示出来，效果如下，绿色的就是检测出来的圆心。

五、标定
确保相机和雷达的都完成了检测，终端不再一直报warnning之后（偶尔报没有关系），便可以开始标定，就是根据两个传感器检测结果来迭代求取外参。
再再新开打开终端输入

source devel/setup.bash
roslaunch velo2cam_calibration registration.launch sensor1_type:=stereo sensor2_type:=lidar

一直输入Y就行，直到迭代完成，会询问你是否换一个姿态继续。
作者告诉我们至少要三个姿态，可以继续重复步骤三-五直到不想继续了。

检测结果

在完成标定之后会立刻在终端显示标定结果，也就是一个tf，具体的还可以在~/catkin_ws/velo2cam_calibration/launch/calibrated_tf.launch中查看，他给了三个坐标系之间的静态tf，但我们需要的是lidar到camera的。
那这个tf应该怎么使用呢？可以参考这篇文章，将这两个tf得到外参矩阵。
订阅TF消息，并转化为齐次变换矩阵
使用得到的外参矩阵和内参矩阵，可以通过投影的方法测试效果，具体可以参考文章：
ROS中点云学习（七）：激光点云和图像的融合
最终效果如下

总结

使用这个算法还是花费了很长时间，我主要遇到的坑列在下面：

1. 标定板的标志一定要对应；
2. 相机同步问题，如果不行一定要用相同的时间戳；
3. 过滤点云要保留洞透过去的点；
4. 最后得到的tf要转化为齐次矩阵来检验下过。