Automatic extrinsic calibration between a camera and a 3D Lidar using 3D point and plane corresponde
Automatic extrinsic calibration between a camera and a 3D Lidar using 3D point and plane corresponde 论文阅读笔记
论文地址：https://arxiv.org/abs/1904.12433
一、Problem Statement
外参标定需要从具有不同模态和噪声模式的不同传感器获得目标特征，而这些噪声特征降低了标定的准确性。此外，并非所有的激光雷达或者相机都具有相似的特性和测量误差，比较难泛化标定方法。
二、Direction
- 选择不易受噪声影响的特征。
a. 使用标定板的法向量
b. 标定板的中心点 - 使用鲁棒的优化策略。
a. 使用遗传算法(Genetic Algorithm) - 使用相机和16线激光雷达。
- Target base 标定方法。
三、Method
给定 N N N个 c a m e r a a − l i d a r l camera_a - lidar_l cameraa−lidarl样本中的第 i t h i^{th} ith样本，从中提取标定板的中心点 ( O c / l i ) (O_{c/l}^i) (Oc/li)和法向量 ( n c / l i ) (n_{c/l}^i) (nc/li)。法向量能够定义标定板平面的旋转，中心点能够定位标定板的3D位置。因此，我们就可以从这些特征中，寻找他们的对应关系。但之前，我们需要知道以下信息：
● ( l , w ) (l,w) (l,w): 标定板的长度和宽度，以米为单位。
● ( m , n ) (m,n) (m,n): 标定板格子的大小，即每行每列内角的个数。
● ( s ) (s) (s): 标定板每个方格的长度大小， 以米为单位。
● ( b b x m n , m x , b b y m n , m x , b b z m n , m x ) (bb_{x_{mn,mx}},bb_{y_{mn,mx}}, bb_{z_{mn,mx}}) (bbxmn,mx,bbymn,mx,bbzmn,mx): 3D雷达ROI区域， x , y , z x,y,z x,y,z三个轴向。
● ( f x y , c x y , d 1 , 2 , 3 , 4 , 5 ) (f_{xy}, c_{xy}, d_{1,2,3,4,5}) (fxy,cxy,d1,2,3,4,5): 相机内参，包括焦距，畸变中心，和畸变系数。
1. 特征提取
(1) 激光雷达特征提取
( b b x m n , m x , b b y m n , m x , b b z m n , m x ) (bb_{x_{mn,mx}},bb_{y_{mn,mx}}, bb_{z_{mn,mx}}) (bbxmn,mx,bbymn,mx,bbzmn,mx): 能够提供ROI区域，先剪切 Z Z Z区域范围，直到只有标定板的平面。下一步就是使用RANSAC去拟合平面，得到其法向量 n l i n^i_l nli。为了得到中心点，计算每条水平线的 y y y方向的最大点和最小点来判断边界点。然后拟合4根线且计算他们的交叉获得四个角点，则四个角点的对角位置就是标定板的对角。
中心点不会受到板方向变化的影响，但会受激光雷达测量误差以及边缘和角点误差的影响。 所以作者使用了中心点作为特征。
(2) 相机特征提取
使用OpenCV的cv::findChessBoardCorners函数和 PnP算法。将标定板的角点位置和相机内参输入进PnP中。对于鱼眼相机模型，首先需要矫正原始图像的畸变后，再获取标定板的角点，然后输入0畸变进入PnP算法之中。PnP的输出是旋转和平移位姿，即 O c i O_c^i Oci。旋转矩阵的第三列，代表着 Z − a x i s Z-axis Z−axis的旋转，其他两个轴在平面上，垂直于 Z Z Z轴。除此之外，通过找到角点和已知的标定板大小 ( l , w ) (l, w) (l,w)，可以得到标定板的中心点。
2. 优化算法
获得了需要的特征之后，通过遗传算法（GA），来获得变换矩阵。由于其随机性，遗传算法“进化”为更好的解决方案，而不容易在高度非凸状态空间中陷入收敛到局部极小值的陷阱。 本文优化的变量是从雷达到相机的基于欧拉角的旋转，和平移。
θ l c = [ θ x , θ y , θ z ] ， t l c = [ x , y , z ] \theta_l^c = [\theta_x , \theta_y, \theta_z]，t_l^c = [x, y, z] θlc=[θx,θy,θz]，tlc=[x,y,z]
为了限制这些变量的搜索范围，给定了一个初始的估计值 ( R ^ l c ( θ ^ l c ) , t ^ l c ) (\hat{R}_l^c(\hat{\theta}_l^c), \hat{t}_l^c) (R^lc(θ^lc),t^lc)，如下：
R ^ l c N l = N c − − − − − − − − − − − N l T ( R ^ l c ) T = N c T − − − − − − − − − − − N l N l T ( R ^ l c ) T = N l N c T − − − − − − − − − − − ( R ^ l c ) T = ( N l N l T ) − 1 ( N l N c T ) − − − − − − − − − − − R ^ l c = ( ( N l N l T ) − 1 ( N l N c T ) ) T \hat{R}_l^c N_l = N_c \\ -----------\\ N_l^T(\hat{R}_l^c)^T = N_c^T \\ -----------\\ N_lN_l^T(\hat{R}_l^c)^T = N_lN_c^T\\ -----------\\ (\hat{R}_l^c)^T = (N_lN_l^T)^{-1}(N_lN_c^T) \\ -----------\\ \hat{R}_l^c = ((N_lN_l^T)^{-1}(N_lN_c^T))^T R^lcNl=Nc−−−−−−−−−−−NlT(R^lc)T=NcT−−−−−−−−−−−NlNlT(R^lc)T=NlNcT−−−−−−−−−−−(R^lc)T=(NlNlT)−1(NlNcT)−−−−−−−−−−−R^lc=((NlNlT)−1(NlNcT))T
其中， N l , N c , O l , O c N_l, N_c, O_l, O_c Nl,Nc,Ol,Oc分别代表表 3 × N 3 \times N 3×N矩阵的列向量 n l i , n c i , o l i , o c i n_l^i, n_c^i, o_l^i, o_c^i nli,nci,oli,oci， i = 1 , 2 , 3 , . . . , N i = 1, 2, 3, ..., N i=1,2,3,...,N样本。
先来看一下总的优化方程:
R ^ l c ( θ ^ l c ) = arg min ( R l c ( θ l c ) , t l c ) e t + v t + e d + e r + k e t , I s . t . θ l c ∈ θ ^ l c ± π / 18 t l c ∈ t ^ l c ± 0.05 \hat{R}_l^c(\hat{\theta}_l^c)=\argmin_{(R_l^c(\theta_l^c), t_l^c)} e_t +v_t + e_d + e_r + ke_{t, I} \\ \quad \\s.t. \quad \theta_l^c \in \hat{\theta}_l^c \pm \pi/18 \\ \quad \quad t_l^c \in \hat{t}_l^c \pm 0.05 R^lc(θ^lc)=(Rlc(θlc),tlc)argminet+vt+ed+er+ket,Is.t.θlc∈θ^lc±π/18tlc∈t^lc±0.05
对上面优化旋转矩阵之后，平移矩阵通过下面来估计:
t ^ l c = m e a n ( O c − R ^ l c O l ) \hat{t}_l^c = mean(O_c - \hat{R}_l^c O_l) t^lc=mean(Oc−R^lcOl)
下面分别讲解一下上面的每一项.
● e t e_t et是激光雷达点云求处的中心点，通过投影到图像坐标系下，与图像所求的中心点进行一个欧几里得距离误差均值。
e t = 1 N { ∑ i = 1 N ( ∑ ( o c i − o l , c i ) 2 ) } e_t = \frac{1}{N}\bigg\{\sum_{i=1}^N \bigg(\sqrt{\sum (o_c^i - o_{l,c}^i)^2}\bigg)\bigg\} et=N1{i=1∑N(∑(oci−ol,ci)2)}
其中 o l , c i = （ R l c o l i + t l c ） o_{l,c}^i = （R_l^co_l^i + t_l^c） ol,ci=（Rlcoli+tlc），即雷达中心点投影到相机坐标系下的中心点。
● v t v_t vt是是激光雷达点云求处的中心点，通过投影到图像坐标系下，与图像所求的中心点进行一个欧几里得距离误差方差。
e t = 1 N { ∑ i = 1 N ( ∑ ( o c i − o l , c i ) 2 − e t ) 2 } e_t = \frac{1}{N}\bigg\{\sum_{i=1}^N \bigg(\sqrt{\sum (o_c^i - o_{l,c}^i)^2} - e_t\bigg)^2\bigg\} et=N1{i=1∑N(∑(oci−ol,ci)2−et)2}
这个方差项是用来避免不同样本之间的bias。
● e d e_d ed是相机坐标系下，中心点和角点的向量与雷达在相机坐标系下投影的法向量点积。
e d = 1 N { ∑ i = 1 N ( ( o c i − o ˉ c 1 i ) ⋅ n l , c i ) 2 } e_d =\frac{1}{N}\bigg\{\sum_{i=1}^N((o_c^i - \bar{o}_{c1}^i) \cdot n_{l,c}^i)^2\bigg\} ed=N1{i=1∑N((oci−oˉc1i)⋅nl,ci)2}
如果是变换的 n l , c i = ( R l c n l i ) n_{l,c}^i = (R_l^cn_l^i) nl,ci=(Rlcnli)与向量是垂直的关系，这个点积应该是为0的。
● e r e_r er是法向量对齐误差。
e r = 1 N { ∑ i = 1 N ( ∑ ( n l , c i − n c i ) 2 ) } e_r =\frac{1}{N}\bigg\{\sum_{i=1}^N\bigg(\sqrt{\sum(n_{l,c}^i-n_c^i)^2} \bigg)\bigg\} er=N1{i=1∑N(∑(nl,ci−nci)2)}
● e t , I e_{t, I} et,I是中心点的投影误差。
e t , I = max { ∑ ( o c , I i − o l , c , I i ) 2 } e_{t,I} = \max\bigg\{\sqrt{\sum(o_{c,I}^i - o_{l,c,I}^i)^2}\bigg\} et,I=max{∑(oc,Ii−ol,c,Ii)2}
上面的优化方程有一个参数 k = s p l k=\frac{s}{p_l} k=pls，用于表示像素到米的一个转化误差。
四、Conclusion
联合标定算法，优化理论，比较重要，需要看看代码。