基于LM 算法的运动相机与激光雷达联合标定方法

李红帅, 罗笑南, 邓春贵, 钟艳如

(1.桂林电子科技大学 计算机与信息安全学院,广西 桂林 541004;2.桂林电子科技大学 广西图像图形与智能处理重点实验室,广西 桂林 541004)

运动相机与三维激光雷达组成的多传感器系统作为实现室内场景三维重建和室外数字化摄影测绘重要的技术手段,目前正被广泛研究和应用。图像数据包含丰富的颜色纹理信息,激光点云数据提供了物体高精度三维位置信息,将两者进行数据融合可以优势互补,实现室内环境三维重建。由于两传感器采集的数据均是在各自坐标系下的表示,只有准确获取两传感器之间的联合标定参数矩阵,才能更好地实现数据融合。

两传感器间联合标定是利用标定板角点所对应激光点云三维坐标和图像二维坐标来构造参数方程,进而求解出两传感器间的联合标定矩阵。常用标定板的特征角点有点、直线和平面等特征,其中利用平面特征方法可以避免出现混合像素现象,因而被研究者广泛采用。Ishikawa等[1]提出一种无目标的,利用传感器融合里程计实现相机-激光雷达自动标定方法。Kang等[2]提出一种无需人造目标即可精确估计相机与激光雷达之间刚体变换联合标定算法,通过对2个传感器之间边缘对齐成本函数求最小化极值来估算校准参数。Huang等[3]提出一种确定激光雷达与单目相机间外部校准新方法,通过具有特殊形状的标定物减少激光雷达量化和系统误差,改善目标姿态估计,实现激光雷达点云投影到图像相应角点上。韩正勇等[4]利用相机坐标系和激光雷达坐标系下棋盘面数据对应性,将外参标定转换为三维空间中旋转、缩放矩阵求解问题。俞德崎等[5]利用Ar Uco标签确定标定板在激光点云坐标系与相机坐标系间关系,通过随机采样一致性算法提取标定板角点坐标并建立对应关系方程,通过Kabsh算法求解配准参数。唐庆博等[6]利用单点激光替换激光测距仪,通过激光-光心共线方程实现高精度外参矩阵求解。魏克全等[7]利用传感器测量原点到靶标平面的距离,构建对应方程,并提出一种基于重投影误差的计算方法,通过优化靶标摆放位置来减小位置随意性对联合标定结果的影响,但其联合标定参数精度还有待提高。

鉴于此,在文献[7]的基础上对运动相机与三维激光雷达联合标定进行优化,提出了一种基于LM算法的运动相机与激光雷达联合标定优化方法。通过运动相机与激光雷达实际联合标定实验,验证了该优化方法的准确性和有效性,为实现激光点云与图像数据之间的融合以及推动数字摄影测绘领域的发展提供了依据。

1 联合标定模型

实现运动相机与激光雷达间的联合标定[8-13],首先需将标定板放置在激光雷达和运动相机共同观测范围内,通过激光雷达和运动相机分别采集标定板角点所对应的三维激光点云坐标和图像像素坐标,构建方程组求解出联合标定矩阵。运动相机与三维激光雷达间联合标定模型如图1 所示。其中,OWXWYWZW表示激光雷达坐标系,OC-XCYCZC为运动相机坐标系,R和T分别表示2个坐标系之间的旋转矩阵和平移矩阵。

图1 运动相机与激光雷达联合标定模型

运动相机坐标系与激光雷达坐标系之间的变换关系为

1.1 运动相机模型

运动相机是超广角相机,但其符合针孔相机模型。首先以OW为局部空间坐标系原点,建立OWXWYWZW局部空间三维坐标系,其表示物体在局部三维空间坐标系下的三维坐标,此处可表示三维激光点云坐标。然后以OC为原点,建立OC-XCYCZC运动相机像空间坐标系。最后以O′为原点,建立O′-xyz图像的像平面坐标系,其中原点O′为相机的像主点,z轴与ZC轴重合。运动相机模型如图2所示。

图2 运动相机模型

根据运动相机针孔成像模型原理,图像像素坐标系与相机坐标系之间的关系为

其中:(u,v)为空间某点P在像平面投影点p的像素坐标;f为运动相机焦距;(u0,v0)为像平面主点像素坐标;M1为运动相机的内参矩阵;s为比例因子。

局部三维空间坐标系与运动相机空间坐标系之间的变换关系使用齐次坐标表示为

其中:R和T分别表示局部空间坐标系到运动相机坐标系之间的旋转矩阵和平移矩阵;M2为激光雷达与运动相机间的联合标定矩阵。

由式(2)、(3)可得,局部三维空间坐标系下点P与其投影点p之间的关系为

其中,M表示M1与M2的乘积,是局部三维空间坐标系到图像像素坐标系之间的旋转平移矩阵的符号表示。

1.2 图像畸变校正原理

超广角运动相机采集的图像具有一定畸变,需对图像进行畸变校正。基于OpenCV 3.0的鱼眼相机畸变校正原理[14-15]对图像畸变校正。

根据针孔相机模型,若求解局部三维空间坐标系下点P在图像像素坐标系下的坐标,需将像平面系下坐标做归一化处理,得到像素坐标系下坐标:

其中,(x,y,z)为点P在像平面坐标系下的坐标。

然后将图像平面模型下图像的像素点坐标转换为鱼眼半球模型下极坐标:

其中:(u,v)为畸变校正前图像的像素坐标;(r,θ)为在鱼眼半球下的极坐标。

获得鱼眼半球下的极坐标后,需通过Gopro运动相机鱼眼失真误差模型进行计算,误差θd的求解公式为

其中,k1、k2、k3、k4为运动相机畸变参数,可通过对运动相机进行畸变校正获得。

利用误差θd将三维空间点坐标重投影到二维图像平面,得到畸变校正后的点坐标(x′,y′),

利用畸变校正后的激光点云坐标求解出畸变校正后的像素坐标(u′,v′),

最后通过双线差值法将像素坐标(u,v)对应图像的RGB颜色值赋给畸变校正后的像素坐标(u′,v′),得到畸变校正后的标定板图像。

1.3 激光点云预处理

激光雷达采集激光点云数据后,需对激光点云进行预处理操作,进而为激光雷达与运动相机联合标定奠定坚实的基础。激光点云预处理包括点云滤波、点云配准、点云分割及求标定板点云中心和点云角点坐标等操作。在实际项目中,为提高激光雷达与运动相机联合标定精度,须准确获取标定板点云中心坐标和标定板各个角点坐标。通过人工手动操作获取标定板点云中心坐标不仅费时费力,而且准确度不高。因此,采用一种手动与自动相结合的点云分割方法实现对目标场景的点云分割,并通过点云中心迭代算法求得标定板点云中心及各角点坐标。激光点云预处理流程如图3所示。

图3 点云预处理流程

激光雷达采集的激光点云需通过点云滤波操作对场景点云中存在的噪声点进行去噪,再通过点云配准将不同姿态下激光点云坐标统一到一个坐标系下,进而通过手动操作对配准后点云进行点云分割,获得包含标定板的激光点云场景,最后通过Kmeans聚类算法[16]进行自动拟合处理,获得准确含标定板的点云。

Kmeans聚类算法具体实现步骤如下:

首先从标定板点云数据集中随机选取k组并初始化为点云聚类中心Ci(1≤i≤k),计算其他众多数据对象点云与聚类中心Ci的欧氏距离,找出离目标数据对象最近的聚类中心Ci,并将数据对象点云分配到聚类中心Ci对应的点云簇中。然后计算每个点云簇中数据对象点云的平均值并将其作为新的聚类中心,再进行下一次迭代更新,直到聚类中心不再变化或达到最大的迭代次数,则停止迭代更新。空间中数据对象点云与点云聚类中心间的欧式距离为

其中:x为数据对象点云;Ci为第i个聚类中心;m为数据对象点云的维度;xj、Ci,j分别为x、Ci的第j个属性值。

整个数据集的误差平方和为

k为激光点云簇个数,fSSE的大小表示聚类算法结果的优劣。

经Kmeans聚类算法对激光点云处理后,标定板激光点云周边不属于标定板的点云大多数会被自动去除,最后只剩下包含标定板的激光点云。

因激光雷达硬件结构特点,导致采集的点云数据存在一定误差。激光点云经预处理后,仍存在特别少的离群点偏离棋盘格标定板平面,此时若直接通过求均值方法求解出标定板点云中心点坐标和各个角点的点云坐标,其求解结果会不准确,影响运动相机与激光雷达之间的联合标定精度。

为提高两传感器间的联合标定精度,基于点云聚集思想,通过点云中心迭代算法求解出准确的标定板点云中心坐标以及各个角点坐标。点云中心迭代算法在保证点云特征不变的同时,通过缩小点云空间范围,进而有效降低离群点带来的误差影响。点云迭代算法详细步骤如下:

1)对于点云集Q={q1,q2,…,qk},通过求均值的方法计算标定板的初始中心点坐标:

其中:k为激光点云集Q中三维激光点云的数目;qi为第i个激光点坐标;q-为这k个激光点云坐标的均值。

2)计算点云集各激光点云坐标与该点云中心之间的距离总和:

3)利用各个激光点云坐标与点云中心间距离总和Ds对激光点云集Q更新,计算得到更新后点云集Q′={q1′,q2′,…,qk′},

4)对Q′={q1′,q2′,…,qk′}再次通过求均值的方法计算更新后的标定板中心点云坐标:

其中,εq为迭代误差阈值。

6)若q-与q-′之间的差值小于迭代误差阀值,则q-′为该点云集的中心,否则,继续按式(12)～(15)进行迭代计算。

求出激光点云中心坐标后,需对标定板的各个角点坐标进行计算。根据标定板实际尺寸、标定板每行每列黑白方块个数及求解出的标定板点云中心点坐标,计算出标定板各个角点的点云坐标。

2 联合标定

2.1 求解初值

要实现激光雷达与运动相机之间的联合标定,需通过多组标定板点云角点坐标和相对应的图像角点坐标求解得到。首先基于直接线性变换法(DLT)利用图像角点坐标与相对应的激光点云坐标构建多组线性方程组,对多组线性方程组进行求解,得到两传感器之间联合标定初值。由式(2)～(4)可得

其中,ti为待求变量,每个特征点提供了2个关于ti的线性约束。

对n个特征点,可得2n个关于ti变量的线性方程。由于ti共有12个变量,至少需6对匹配点才可实现对所有变量的线性求解。求解出的所有变量组成的矩阵就是激光雷达与运动相机间的联合标定矩阵初值。

2.2 LM(Levenberg-Marquardt)优化算法

LM 优化算法[17-20]是一种经过多次验证,可以有效解决非线性最小二乘函数的经典算法。该算法集成了高斯牛顿法和梯度下降法2种算法各自的优势,通过控制改变阻尼因子变量的值,即通过调节每次算法迭代的步长,有效提升迭代优化效率,实现对目标函数最小二乘法优化,求出最佳的旋转平移矩阵。

由直接线性变换法求出运动相机与激光雷达之间的联合标定矩阵初值后,对其进行非线性优化。对于n个标定点,可通过最小化目标函数对求解的联合标定初值进行优化:

其中:(xd,i,yd,i)分别为第i个标定板角点点云坐标经过点云重投影到标定板平面的坐标;(ui,vi)为第i个标定板角点所对应的实际坐标。

由于LM 算法集成了最速下降法和高斯牛顿法各自的优点,可通过试探步dk来进行调节。对式(19),高斯牛顿法求解问题的试探步dk用函数表示为

dk=-((J(xk))TJ(xk))-1J(xk)F(xk), (20)

其中,J(xk)为F(x)关于x的导数(雅克比矩阵),xk为函数其中的一个解。

高斯牛顿法要求J(xk)矩阵为行满秩矩阵,故无法使用J(xk)矩阵求解非线性方程组相关奇异问题。LM 算法对高斯牛顿法进行改进,通过引入一个正定矩阵uI,并将其加到(J(xk))TJ(xk)上,使之成为正定矩阵。此时LM 算法的迭代步长为

其中:I为单位矩阵;u为LM 参数。当u→0时,dk趋向于高斯牛顿法,当u→∞,dk趋向于最速下降法。

将直接线性变换法求得的联合标定矩阵作为函数初值,且取k1、k2、k3、k4初值均为0,进行迭代优化求解。LM 算法实现详细步骤如下:

1) 输入初值x0,初始化参数u=0.01,设定精度ε=1×10-8;

2) 计算F(x)和J(x)的值;

3) 计算dk,则更新后,Xk+1=Xk+dk;

4) 若F(xk+1)＜F(xk),且‖dk＜ε‖,则停止迭代,并输出优化后联合标定结果;否则,u=u/2,转至步骤2),继续循环;

5) 若F(xk+1)≥F(xk),则u=2u,重新计算dk的值,返回到步骤4),继续循环,直到满足迭代终止条件,则停止循环。

3 实验

采用Velodyne 16线激光雷达(VLP-16)和运动相机(Gopro Hero5 session)这2种传感器在实际项目中进行联合标定及数据融合实验。其中VLP-16激光雷达和Gopro Hero5 session相机的详细参数分别如表1、表2所示。

表1 VLP-16激光雷达参数

表2 Gopro Hero5 session相机参数

首先基于张正友标定法原理对运动相机进行内参标定,将棋盘格标定板贴在平整平面上,再基于MATLAB软件的内参标定模块进行运动相机内参标定。相机内参标定如图4所示。运动相机经过内参标定后,内参标定结果如表3所示。

图4 相机内参标定

表3 运动相机内参标定结果

在进行激光雷达与运动相机联合标定实验时,采用5×8印有黑白棋盘格的平面标定板,材质为PVC材料,尺寸为20 cm×32 cm。为提高运动相机与激光雷达之间的联合标定精度,首先让标定板都处于两传感器公共视野范围内,并尽可能地增大激光雷达扫描时落在标定板上激光点云的数量。经实验,当标定板与激光雷达和运动相机的水平距离在1～2 m 时,激光雷达扫描落在标定板上的激光点云的条数和数量最多,运动相机采集的图像也可通过OpenCV 清晰地对其标定板图像的角点进行识别和检测。

通过改变标定板位置,以运动相机光轴为对称轴,对称采集20个不同位置下含标定板的激光点云数据和图像数据,并对这些数据进行预处理操作。对图像的预处理是对运动相机采集的图像进行畸变校正操作。畸变校正前后对比如图5所示。

图5 图像畸变校正前后对比

对激光点云的预处理操作包括点云滤波、点云配准、手动与自动相结合的方法进行点云分割及通过点云中心迭代算法求出标定板点云中心坐标和各个角点坐标。激光点云经过点云滤波可去除激光点云中的离群点和噪声点。滤波前后点云如图6所示。

图6 滤波前后点云

由于采集的是静态环境下的标定板激光点云,都处于同一点云坐标系下,无需进行点云配准操作。点云分割采用的是手动与自动相结合的方法对标定板点云进行分割,其中一个标定板位置下点云分割前场景如图7所示。

图7 点云分割前场景

首先通过PolyWorks 2019软件对原始三维激光点云进行手动分割操作,手动分割出含标定板的激光点云。然后通过Kmeans聚类算法对含标定板的激光点云进行自动处理,得到仅含标定对象且边界清晰的标定板点云。最后基于点云迭代算法求解出标定板点云的中心坐标和各个角点对应的点云坐标。标定板点云拟合图如图8所示,激光雷达与运动相机联合标定流程如图9所示。

图8 标定板点云拟合图

图9 激光雷达与运动相机联合标定流程

通过运动相机内参矩阵和多组标定板点云角点坐标及其对应的图像角点坐标,基于直接线性变换法原理构造多组线性方程组,并对线性方程组求解,得出激光雷达与运动相机联合标定初值。通过对LM算法引入正定矩阵和改变迭代步长的方法进行迭代,求解出优化后的联合标定矩阵。LM 优化算法前后结果对比如表4所示。

表4 LM 算法优化前后结果对比

迭代优化完成后,使用优化后的联合标定矩阵进行点云重投影,并计算图像实际角点的像素坐标与激光点云重投影至图像像素坐标下角点之间的误差,即重投影误差。由于采用的运动相机焦距只有2.77 mm,分辨率为2 736×3 648,经过LM 算法优化后重投影误差与初值相比,垂直方向误差降低了7个像素,水平方向误差降低了8个像素,重投影误差降低了约35%。标定板图像角点识别和点云重投影角点如图10所示,图中,红色圆框表示经OpenCV 进行图像识别的角点,蓝色的表示点云重投影的角点位置。经过LM 算法优化后,重投影误差明显降低,点云重投影优化前后对比如图11所示,优化后重投影到标定板的激光点云更清晰。

图10 标定板图像角点识别和点云重投影角点

图11 点云重投影优化前后对比

要实现激光点云与图像融合,需基于共线方程原理并通过图像像素点与点云单应性计算方法求解出三维激光点云坐标所对应的图像像素坐标,并将图像像素坐标处的RGB值赋给相应的激光点云,最终实现激光点云与图像融合的三维重建[21-23]。三维室内环境重建效果如图12所示。

图12 三维室内环境重建效果

4 结束语

针对运动相机与激光雷达之间的数据匹配问题,设计了一种基于LM 算法的运动相机与激光雷达联合标定优化方法。实验结果表明,利用多组标定板点云角点坐标和对应的图像角点坐标,通过直接线性变换法求解出两传感器间联合标定初值,经LM 优化算法迭代求解后,得到优化后的联合标定结果。利用该联合标定结果,将激光点云重投影至图像,与联合标定初值相比,其重投影误差降低了约35%。最后利用联合标定结果并基于共线方程原理实现了激光点云与图像融合的三维重建,为数字摄影测绘领域发展提供了依据。