前段时间尝试了一款激光雷达和相机标定的代码，总结了博客：
【学习总结】激光雷达与相机外参标定：原理与代码

但总觉得那个代码太差劲，而且精度不行，于是又找了些新的代码，体验比之前的好很多，在此做记录。而且精度目测比之前的代码高不少，主要是因为会自动筛选最合适的位姿组合。

代码：https://github.com/acfr/cam_lidar_calibration
论文：Optimising the selection of samples for robust lidar camera calibration
youtube视频：https://youtu.be/WmzEnjmffQU

这个代码的参数配置比之前那个清晰很多，只有一个yaml文件，而且提供的可视化工具也很完善，调试起来解决一些小的接口问题就能够直接运行，好评。

代码使用方法

1. 运行雷达、相机节点，发布雷达数据，图片数据以及camera info数据；
2. 运行主要程序节点，通过rqt_config工具，截取xyz坐标轴，使rviz中的点云尽量只保留棋盘格区域；
3. 点击capture进行一次截取；
4. 更换棋盘格位置或重新载入下一个rosbag，再点击capture；
5. 重复3-4，包括至少3个位姿，可以更多；如果某次截取效果不好，可以discard当次截取；
6. 数量足够后，点击optimize按钮，后台开始优化；
7. 优化结果输出到命令行与指定路径，并可进行可视化显示误差与结果。

开始截取前的界面，黄色圆圈部分是棋盘格的点云；

修改1中的区间，使仅保留棋盘格点云如2所示，再点击3处的capture

如果成功检测，节点会发布图像中检测的棋盘格和角点，同时点云的窗口会框出棋盘格边界线。如果标定的不准，可以放弃这次截取。

再开始下一次截取。可以后台更换rosbag，所以原始数据可以每次保持静止，搞一个rosbag后调整位置，再录制。

点击optimise后，后台开始优化。后台会将所有位姿任选3个进行组合，计算voq得分，然后选取得分最高的一组，进行后续优化。关于voq的含义查看论文。由于这次只录制了3组，所以只有1种组合。优化后的结果是“旋转向量+平移向量”形式，目测是从Lidar系到camera系的变化。其中旋转向量可以转成XYZ欧拉角，如下：

代码原理

代码原理也比较简单，虽然代码很多，但需要简单查看就可以发现主线很是清晰：

1. 接收图片和点云数据
最开始程序会接收image和雷达pc两个数据，并通过message_filter进行时间上的同步，所以务必要保证雷达点云和相机的时间戳是基本同步的。同步接收后，会进入extractRegionOfInterest回调。

进入回调后，会首先对点云进行滤波即根据rqt设定的动态参数截取棋盘格区域。之后当点击 capture 按键后进入flag内的代码，进行一次请求。

对于图像检测chessboard就没啥特殊的了。这里重点关注一下点云的处理：首先根据雷达扫描的ring的信息，提取每条ring的最大y值和最小y值，然后第一个和最后一个作为这条ring的起止点。所有的ring处理完后，就得到了棋盘格的四条边。之后四条边进行ransac拟合直线，再计算棋盘格角点。
可以看出，这种计算方法：1）需要雷达具备ring信息（如果不具备请参考上一篇帖子：【将镭神C32激光雷达的PointXYZ数据转化为PointXYZIR格式 - 附代码】；2）要求这条ring不要扫到非棋盘格的点，即上一步进行区域筛选时不要有非棋盘格以外的点。

为了达到 2）这个要求，棋盘格一般与周围物体分离。例如代码作者采用三脚架固定：

而我这边则是“挂起来”：

注意事项

1. 使用时需要按照代码要求，修改参数以及topic的名称；
2. 如果相机不能自己发布camera_info消息，则需要手动发送。手动造camera_info的方法之前也踩过坑：【ROS中生成CameraInfo消息】
3. 如果雷达不具备ring的信息，则需要手动添加。手动计算ring信息的方法前连天踩的坑：【将镭神C32激光雷达的PointXYZ数据转化为PointXYZIR格式 - 附代码】
4. 注意但凡涉及了点云，需要将frame_id设置成一致的，否则容易出问题；

附我这边运行时的 rqt_graph 示意：