2d Laser 和 camera 标定工具原理及使用方法

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/heyijia0327/article/details/85000943

2d 激光和相机之间的标定早在 04 年就出了成熟的论文和方法,17 年 ICCV, IROS,今年 IROS 等依然还有论文产出,具体的论文列表可以参考我的《论文阅读整理》博客。本篇博客主要讲述 04 年方法的原理以及代码的实现, 代码开放在我们的github上,在自己机器人上采集了多次数据进行标定,标定结果在 1cm 内浮动,均值附近正负几毫米。

标定原理

准备材料:标定板一个(平板上贴有棋盘格或者二维码,我使用的为 apriltag,apriltag 使用我们代码文件夹里提供的,直接打印成 A4 大小,特别注意自己在网上下载的 apriltag 大小各异,注意测量 apritag 的尺寸,我们的为0.1612m),已经标定好内参数的相机一个。

标定原理如下图所示,相机通过二维码估计标定板平面在相机坐标系下的平面方程,由于激光点云落在平面上,将点云通过激光坐标系到相机坐标系的外参数 TclT_{cl} 转换到相机坐标系,构建点到平面的距离作为误差,使用非线性最小二乘进行求解。
在这里插入图片描述

标定板平面在相机坐标系下的求解

假设通过二维码求解 Pnp 已经知道二维码到相机坐标系的变换矩阵 TcmT_{cm}, 平面方程在二维码坐标系下为 Pm=[0,0,1,0]P_m = [0,0,1,0]^\top。那么平面方程在相机坐标系为
Pc=(Tcm1)PmP_c=(T^{-1}_{cm})^{\top}*P_m平面方程在不同坐标系下的变换的公式推导,可以参看 stackoverflow 上别人的回答。

误差方程的构建和求解

平面在相机坐标系下的方程知道后,就能很快构建误差方程, 首先将激光点 plip^i_l 从激光坐标系转换到相机坐标系。pci=Tclplip^i_c = T_{cl} * p^i_l 然后,利用点到平面的距离方程构建误差,ri=d(pci,Pc)r_i = d(p^{i}_c,P_c) 使用 ceres 对一堆点的误差进行非线性最小二乘求解。

改进

实际上标定时会采集很多时刻的数据,激光点云数据量加起来可能会很多,直接丢给 ceres 求解速度慢。对于某一时刻,由于点云噪声通常在几厘米跳动,所以我们可以先对该时刻的激光数据进行直线拟合,然后只利用直线上的两个点来构建约束,这样就能减少 ceres 的计算量。

标定工具的使用

这类工作原理简单,但是网上没有太好的代码集成,好多还是基于 matlab 。因此,我按照上述原理结合 apriltag 姿态估计整合了一个简单易用的 ros 工具,只需要你准备好 rosbag 数据,然后框框几下激光数据就能完成标定,代码开放,免得重复造轮子。

1. 数据的采集

准备一个如图所示的标定板,A4 纸打印一个 Apriltag, 贴在一个平板上。然后将标定板至于相机和激光前方(0.5m ~ 1.5m 之间),激光线要能落在标定板上。不断调整姿态,你可以把标定板立在地上来变换不同姿态(这样能更好的保持静止),比如标定板 a 边立在地上,也可以标定板 b 边立在地上,甚至可以标定板的顶点立在地上,标定板也要各种倾斜,比如前倾,左倾,后仰等等(而不是一直竖着拿着),一定要有充分的各轴激励,才能完成好的标定。每换一个姿态,请保持静止 2 秒以上,采集 12 个姿态左右的数据(当然越多越好),用 rosbag 记录所有的图像和激光数据, 标定工具会自动检测你保持静止时刻的数据,并用来标定。
在这里插入图片描述

2. 准备好配置文件

运行程序前,需要你简单配置下参数,所有的参数都在 config_and_tool/calibrat_config.yaml 里。
savePath:是指保存相机 pose 的路径。
bag_path:保存的 ros bag 路径。

3. 获取 Marker 在相机里的姿态

运行工具箱的如下代码

roslaunch apriltags_ros example.launch
rosbag play your_bag.bag

这个代码会把所有的 apriltag 的姿态保存到一个 txt 里,后续标定代码会读取这个txt,并自动检测静止时间段。注意,请务必确认相机内参数是否正确,请将正确的相机内参在 config.yaml 中修改。

4. 运行标定工具

运行标定工具代码如下:

roslaunch lClibra example.launch

标定工具自动读取 rosbag, 不需要使用 rosbag play。运行代码后,工具会自动检测每一个静止时刻,一旦检测到,就会把这个时刻的激光数据绘制成图像,你从图像里用方框选取那些恰好落在标定板上的激光,大致选一下就行,只是别选不是平面上的点。然后按任意键继续,如下图所示:
在这里插入图片描述

5. 标定结果的评估

在文件夹 config_and_tool 中准备了一个 python 可视化工具,会将每个静止时刻的点云数据和平面依次绘制出来,你可以查看激光数据是否恰好落在平面上来评估结果的好坏,另外也可以用 report 的残差大小作为评估依据。下图为某一个时刻的激光点云以及标定板平面示意图。
在这里插入图片描述

展开阅读全文

没有更多推荐了,返回首页