一种简单高效的激光雷达和摄像头的联合标定法（无须标定板）

1. 二维激光雷达和摄像头联合标定原理

理论上激光雷达点要先从世界坐标系经过旋转、平移、缩放等变换转换到相机坐标，再乘以相机内参矩阵转换到图像坐标。但其实二维激光雷达坐标到图像坐标的变换可以更简洁地看成是一个射影变换，二维激光的激光点就相当于俯视图，摄像头拍到的图像可以想象为主视图，而俯视图通过乘以一个单应矩阵变换到主视图的过程就是射影变换。转换的一般形式如下：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\ast \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]（1.1）$$
其中[x y 1]是激光雷达的齐次坐标，[u v 1]是激光转换到图像坐标系后的图像齐次坐标，我们提出h矩阵中的h_9并放到等式左边作为缩放因子λ后，可将公式写成如下形式$$\lambda \ast \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{n}_1& n_2& n_3\\ n_4& n_5& n_6\\ n_7& n_8& 1\end{array}\right]\ast \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]（1.2）$$
上式的n矩阵就是我们需要求出来的变换矩阵参数，怎么求呢，这里需要消去缩放因子λ$$\{\begin{array}{c}\lambda =n_{7}x+n_{8}y+1\\ \lambda \ast u=n_{1}x+n_{2}y+n_3\\ \lambda \ast v=n_{4}x+n_{5}y+n_6\end{array}$$
$$=>\{\begin{array}{c}(n_{7}x+n_{8}y+1)\ast u=n_{1}x+n_{2}y+n_3\\ (n_{7}x+n_{8}y+1)\ast v=n_{4}x+n_{5}y+n_6\end{array}$$
$$=>\{\begin{array}{c}u=n_{1}x+n_{2}y+n_3-n_{7}ux-n_{8}uy\\ v=n_{4}x+n_{5}y+n_6-n_{7}vx-n_{8}vy\end{array}（1.3）$$
假设收集到了k组激光雷达点和对应的图像坐标点，那么（1.3）式又可以写成矩阵的形式：
$$\left[\begin{array}{cccccccc}{x}_1& y_1& 1& 0& 0& 0& -u_1{x}_1& -u_1{y}_1\\ 0& 0& 0& x_1& y_1& 1& -v_1{x}_1& -v_1{y}_1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ x_k& y_k& 1& 0& 0& 0& -u_k{x}_k& -u_k{y}_k\\ 0& 0& 0& x_k& k_1& 1& -v_k{x}_k& -v_k{y}_k\end{array}\right]\ast \left[\begin{array}{c}{n}_1\\ n_2\\ n_3\\ n_4\\ n_5\\ n_6\\ n_7\\ n_8\end{array}\right]=\left[\begin{array}{c}{u}_1\\ v_1\\ ⋮\\ u_k\\ v_k\end{array}\right]（1.4）$$

2. 数据采集方法

从(1.4)式中可以看出，一组数据对应两个n参数，那么要求出n向量至少需要4组数据，组成超定方程求最优解至少需要5组数据，超定方程的求解就是最小二乘了，稍后我会给出matlab代码，这里先说一下我的数据采集方法。

如上图所示，我把相同的几张白纸对折，并在与激光雷达等高的位置画上了黑点，调用opencv可以很快得到黑点的图像坐标（即u v的值），网上可以找相关代码（显示鼠标所在坐标的代码）。再看左边激光雷达的图，图中下半部分那三个三角是不是很明显，这就是我对折的原因，顶点位置就是激光雷达点的坐标（即 x y的值）。我用这样的方式总共收集了6组点来求解向量n，收集时尽量让点的位置交错开。

3. Matlab代码求向量n

x=[x_1 x_2 … x_k];                    % 雷达坐标x轴
y=[y_1 y_2 … y_k];                    % 雷达坐标y轴
u=[u_1 u_2 … u_k];                    % 图像坐标u轴
v=[v_1 v_2 … v_k];                    % 图像坐标v轴
z=[u_1 v_1 u_2 v_2 … u_k v_k]';       % 所有u v依次排下去，即（1.4）式等号右边的向量
% X为公式（1.4）中的左边第一个矩阵
X=[x(1) y(1) 1 0 0 0 -u(1).*x(1) -u(1).*y(1) ;
    0 0 0 x(1) y(1) 1 -v(1).*x(1) -v(1).*y(1) ;
    x(2) y(2) 1 0 0 0 -u(2).*x(2) -u(2).*y(2) ;
    0 0 0 x(2) y(2) 1 -v(2).*x(2) -v(2).*y(2) ;
    x(3) y(3) 1 0 0 0 -u(3).*x(3) -u(3).*y(3) ;
    0 0 0 x(3) y(3) 1 -v(3).*x(3) -v(3).*y(3) ;
    x(4) y(4) 1 0 0 0 -u(4).*x(4) -u(4).*y(4) ;
    0 0 0 x(4) y(4) 1 -v(4).*x(4) -v(4).*y(4) ;
    x(5) y(5) 1 0 0 0 -u(5).*x(5) -u(5).*y(5) ;
    0 0 0 x(5) y(5) 1 -v(5).*x(5) -v(5).*y(5) ;
    x(6) y(6) 1 0 0 0 -u(6).*x(6) -u(6).*y(6) ;
    0 0 0 x(6) y(6) 1 -v(6).*x(6) -v(6).*y(6) ];

n=X\z;              % 向量n = X^(-1) * z  即X矩阵的逆乘以z向量

4. 坐标转换的C/C++代码

求得向量n后，就可以在自己的程序里对所有点进行转换了

//我们用matlab算出来的n参数，如下是我的参数
const float n[9] = {-7.2,-4.4,264.8,-0.021,-1.9,1454,-0.00043,-0.0135,1};

for (int i = 0; i < scanData.size(); i++){
       scanDot dot;   
       dot = scanData[i];   //scanData储存了当前激光雷达一圈的数据点

       //因为只有摄像头视野内的激光点能显示，所以定了个范围，不用每个点都去转换
       if(dot.angle > 120 & dot.angle < 240){ 
   			theta = dot.angle*PI / 180;
   			rho = dot.dist;
   			x = (int)(rho*sin(theta));   //激光雷达坐标的x,y通过激光点的距离和角度算得
   			y = (int)(rho*cos(theta));
   
   			lambda = x*n[6] + y*n[7] + n[8];
   			u = int((x*n[0] + y*n[1] + n[2]) / lambda);
   			v = int((x*n[3] + y*n[4] + n[5])  / lambda);
   			
   			circle(camImage, Point(u, v), 1, Scalar(255, 255, 0), -1, 8, 0);//draw lidarpoint
      }
}

转换效果如下

所有的软件工具、流程说明和程序我都打包好做成了个小轮子，感兴趣的读者可以上咸鱼搜索：联合标定。（我使用的激光雷达是2d的rplidar，3d激光雷达无法使用我的方法）