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

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

标定原理

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

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

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

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

误差方程的构建和求解

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

改进

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

标定工具的使用 (具体操作细节请从参考 github readme)

最近升级了代码，使用更加方便，标定结果更加稳定。主要改动如下：

1. 能自动检测落在标定板上的激光点。
2. apriltag 的代码支持利用多个 marker 估计相机到标定板之间的外参数 $T_{cm}$。

1. 数据的采集示例

2. 标定结果的评估示例