一种利用棋盘格进行激光雷达与相机标定方法

1基本原理

1.1相机模型

相机图像坐标系与世界坐标系之间的关系可以表示为：
$$\mathrm{p}\sim \mathrm{K}(RP+\mathrm{t})\text{\hspace{1em}(1)}$$
其中$\mathrm{p}$为图像坐标系坐标，$P$为世界坐标系坐标，$R$表示为相机坐标系与世界坐标系之间的旋转矩阵，$\mathrm{t}$表示为相机光心在世界坐标系在坐标，$\mathrm{K}$表示内参矩阵。
在这里忽略镜头畸变影响，或者已经对镜头畸变进行了处理。

1.2相机与激光雷达之间的关系

设相机坐标系下点$P$在激光雷达坐标系对应的点为$P^f$,则二者有如下关系：
$$P^f=\Phi P+\Delta \text{\hspace{1em}(2)}$$
其中，$\Phi$，$\Delta$分别为相机坐标系与雷达坐标系之间的旋转与平移量。相机与雷达标定的目的即是要求解出$\Phi$，$\Delta$。

1.3几何约束

本文提供的标定方法是在系统前面放置一个平面图案，比如棋盘格，相机和激光测距仪都可以测量到。同时使用相机和激光雷达对平面测量。定义标定平面为棋盘格表面的平面。
在不失一般性的情况下，我们假设标定平面在世界坐标系中其$Z=0$。在相机坐标系中，标定平面可以由3维向量$N$参数化表示，$N$平行于标定平面的法线，其模长$\Vert N\Vert$等于相机到标定平面的距离。由公式（1）我们可以得出:
$$N=-R\left(R^{\top }\cdot \mathrm{t}\right)\text{\hspace{1em}(3)}$$
由于激光雷达点位于标定平面上，可以得到一个几何约束。由公式(2)可以得到：

$P={\Phi }^{-1}\left(P^f-\Delta \right)$。由于位于标定板的相机坐标系下坐标$P$由向量$N$定义，它满足 $N\cdot P=\Vert N{\Vert }^2$。然后可以得到：
$$N\cdot {\Phi }^{-1}\left(P^f-\Delta \right)=\Vert N{\Vert }^2\text{\hspace{1em}(4)}$$
给定一个雷达点$P^f$和向量$N$，即可求解$\Phi$,$\Delta$。

2求解方法

1.1线性方法

假设所有激光点的Y值为0(这里是针对二维激光雷达而言)，激光雷达点$P^f=[X,Z,1]$。公式(4)可以变为：
$$N\cdot H{\hat{P}}^f=\Vert N{\Vert }^2\text{\hspace{1em}(5)}$$
$H={\Phi }^{-1}\left(\begin{array}{lll}1& 0& \\ 0& 0& -\Delta \\ 0& 1& \end{array}\right)$，表示雷达坐标系和相机坐标系之间的关系。对于标定平面的每个旋转和平移而言，可以得到很多关于$H$的线性方程，使用最小二乘可以求解矩阵$H$元素。得到$H$之后，可以得到标定参数：
$$\begin{array}{rl}\Phi & ={\left[H_1,-H_1\times H_2,H_2\right]}^{\top }\\ \Delta & =-{\left[H_1,-H_1\times H_2,H_2\right]}^{\top }{H}_3\end{array}$$
$H_i$表示$H$的列向量。但是这种方法得到的旋转矩阵，不满足正交性，精度不高。可以采用第二种非线性方法求解。

1.2非线性方法

对于不同的标定板的pose（R,t）。定义误差函数：
$$\sum _i\sum _j{\left(\frac{N_i}{\Vert N_i\Vert }\cdot \left({\Phi }^{-1}\left(P_{ij}^f-\Delta \right)\right)-\Vert N_i\Vert \right)}^2\text{\hspace{1em}(6)}$$
$N_i$为标定板在第$i$个pose对应的平面参数。$P_{ij}^f$为第$i$个pose对应的第$j$个激光雷达点坐标。使用线性方法获取标定参数初值，然后利用非线性最小二乘，最小化误差函数，即可求解标定参数。
参考文献：
《Extrinsic calibration of a camera and laser range finder (improves camera calibration》
1