基于3D特征点的激光雷达与立体视觉配准方法 下载: 1501次
1 引言
近年来,无人车行业发展迅速,预计到2021年,无人车将进入市场,进入新的阶段。无人车系统主要分为三个部分:算法端、Client端、云端。其中,算法端的主要任务是利用传感器获取原始数据并从中提取有用的信息,以便了解周围环境状况,进而根据环境具体状况作出判断。无人驾驶中通常使用的传感器包括:全球定位系统(GPS)、惯性测量单元(IMU)、激光雷达、摄像头等。GPS与IMU所获数据的融合是无人车里程计的主要数据来源,激光雷达与摄像头的融合是环境感知、目标检测的主要设备。激光雷达测距远、精度高,但所获信息量少,且无法获得颜色信息,目标检测受限,需将其与视觉信息融合,以形成优势互补。摄像头又可分为单目相机、双目相机、深度(RGB-D)相机。单目相机具有尺度不确定性,有一定局限性。RGB-D相机分辨率低,且只适应于室内环境。双目相机不仅具有单目相机的全部优点,还可以通过视差感知深度信息。因此,激光雷达与双目相机融合具有一定优势,本文主要涉及激光雷达与双目相机的配准。
目前,激光雷达与相机配准的方法主要有两大类:一是先提取点、线等特征集,再通过雷达点云与图像特征之间的匹配,求解两者之间的配准参数[1-5];二是先计算雷达点云与相机图像之间的互信息损失函数,再利用优化算法求解配准参数[6-10],但此类方法只适用于传感器位移及旋转角较小的情况。还有一些方法是将SFM(Structure from motion)算法用于参数配准[11],但需要多个辅助摄像头,硬件成本要求较高。
本文方法是通过提取特征点进行匹配参数求解的,通过简单的实验装置,快速有效地求解激光雷达与双目相机之间的配准参数,可为激光雷达与立体视觉数据融合提供基础。
2 基本原理
求解激光雷达与双目相机之间的配准参数,需要在雷达与相机各自坐标系下提取一组对应的3D点,选取两块木板的八个角点作为待求解的点。双目相机与激光雷达的角点提取采用的是相似的处理流程,即都是通过提取木板边缘3D点并拟合对应的直线,再求取直线交点。下面将介绍算法的具体操作流程。
2.1 双目相机特征点提取
双目立体视觉模拟人类视觉处理景物的方式,融合两只眼睛获得的图像并观察它们之间的差别,使大家可以感知深度。将空间中同一物理点在不同图像中的像素点进行匹配,建立特征间的对应关系,形成视差(Disparity)图。关于视差图计算,有很多种方法,其中最经典的就是SGM(Semi-global matching)算法[12]。可以发现,目前排名靠前的算法很多都是SGM算法的改进,且具有很强的实用价值。综合考虑效果与计算速度,选取SGM算法的一种改进算法:SGBM(Semi-global block matching)算法,采用该方法可以获得较精确的视差图,其算法整体框架如
在获得视差图的基础上,通过相机内参计算出深度,并进而计算出点云坐标(x,y,z)。具体转换原理如
根据三角形相似原理,可求得像素点对应的深度值为
式中:XL-XR为视差值。若左右目相机内参中cX不一致,即cX_L≠cX_R,则需将深度值修改为
式中:doffs为两个相机主点在X方向上的距离,doffs=cX_L-cX_R。
求得的Z为深度值,也即是点云对应的ZC轴坐标,继续求出XC与YC轴坐标,即可获得点云空间坐标,如
式中:cX、cY、fX、fY为相机内参;u、v为待求点云在视差图上的像素坐标。
图 2. 成像系统。(a)视差与深度几何关系;(b)由像素坐标得到空间点
Fig. 2. Imaging system. (a) Geometric relationship between parallax and depth; (b) getting spatial points from pixel coordinates
按照从左到右逆时针的顺序,依次提取图像中木板边缘,根据像素坐标可提取对应空间点云,利用随机抽样一致性(RANSAC)算法拟合点云对应的直线[13-14]。由于存在误差,拟合出的两条空间直线可能不会相交,此时选用直线间最短线段的中点作为待求的角点。
2.2 激光雷达特征点提取
目前,激光雷达的特征提取有很多的方法与应用,如文献[ 15-17]中所采用的方法。本文激光雷达特征点提取选取的方法的提取过程与双目相机类似。默认激光雷达坐标系Z轴朝上、X轴朝前,而双目相机坐标系Z轴朝前、X轴朝右。首先需要通过一个初始旋转矩阵R0将雷达点云坐标轴方向旋转至与双目相机大致相同,方便后续操作,例如先绕X轴旋转—90°,再绕Y轴旋转90°。
关于激光点云,按照文献[ 9]中的方法找到深度间断性过大的点,进而提取出木板边缘。木板平面中的每个点被赋予一个幅度,表示相对于其相邻点的深度差异,并设置一个差异阈值保留木板边缘上的点云。对于空间上的点,较难直接分割出各自在木板边缘对应的点云,而平面上的点云相对容易分割。可以利用所设置的初始旋转矩阵R0与左目相机内参K将过滤后的点云映射到相机左目像素平面上,此过程通过
来计算完成,从而使点云空间坐标与像素点联系在一起,达到选取像素点就能获得对应空间点云坐标的目的。从左到右逆时针依次框选木板的边缘,然后拟合直线,求取角点。其拟合求解顺序与相机边缘一致,目的是便于在两者之间进行角点匹配。
2.3 配准参数求解
2.3.1 Kabsch算法求解过程
用M、N分别表示两组对应的点云,Mi 、Ni分别表示M、N集合内的点。那么优化目标函数为
式中:E为误差函数;R和t分别为待求的旋转矩阵和平移矢量。
对t求偏导,并令等式为零,得
式中:
将(7)式代入优化目标函数(5)式,并令
由(10)式可知,对(5)式求最小值,相当于求Tr(YTX')的最大值。
因为X'=RX,所以有
对XYT作奇异值分解(SVD),即令XYT=UDVT,有
令Z=VTRU,则有
因为R和U都是正交矩阵,所以Z也为正交矩阵,则有det(Z)=±1。
令Zii=1,即Z为单位矩阵,则有
R必须是右手系下的旋转,因此(14)式可修正为
式中:d=sign
由于最初对雷达点云作过一次旋转变换,所以需将此处求解得到的旋转矩阵R乘上初始旋转变换矩阵才能得到最终激光雷达与双目相机的配准参数。
2.3.2 多次配准减少噪声影响
双目相机保持不动,在静止的封闭房间内,通过实时显示双目相机建立的点云,可以发现,点云并不是静止的,来自雷达的点云空间坐标同样也有一定的浮动范围。为了减少噪声影响,在程序运行中需多次估计旋转和平移参数。
经过N次求解,对所有配准参数进行聚类,去除异常值后求取均值,将其作为最终的最优结果。旋转矩阵的均值求解不太容易,且由于四元数可以直接累加,因此可以先把旋转矩阵转换为四元数。四元数与旋转矩阵之间的转换关系如下:
设四元数
反之,假设R=
事实上,一个旋转矩阵对应的四元数表示不唯一,还有其他一些转换方式,且当
3 实验环境搭建
本文所提方法中使用速腾聚创的RS-LiDAR-16激光雷达,相机采用ZED双目相机,主要由两个高分辨率镜头组成,通过Nvidia Jetson Tx2上的GPU加速计算。
实验装置为两块矩形木板,提供8个角点,方便提取特征点对。
图 3. 实验装置图。(a)木板设置图;(b)激光雷达与双目相机放置图
Fig. 3. Experimental device diagram. (a) Board layout; (b) layout of lidar and binocular camera
4 实验结果
通过双目相机提取的木板边缘点云如
图 4. 边缘提取。(a)(b)双目相机不同视角的边缘提取结果;(c)激光雷达点云框选图
Fig. 4. Edge extraction. (a) (b) Edge extraction results of different perspectives of binocular camera; (c) frame selection of lidar point cloud
可以采用三种方法来查看融合结果:一,直接通过联合标定的外参来融合激光雷达与双目相机各自产生的点云;二,通过相机的内参与联合标定的外参将激光雷达点云映射到双目相机的左目图像中;三,通过双目相机与雷达的外参传递,融合两个不同视角的双目点云,从而评估外参的准确性。
4.1 结果验证
方法一:在两种不同场地上的点云融合结果如
图 5. 不同视角下两个场景的点云融合结果。(a)(b)场景一;(c)(d)场景二
Fig. 5. Point cloud fusion results from different perspectives in two scenes. (a) (b) Scene one; (c)(d) scene two
方法二:为了体现效果,将木板上的雷达点云重投影到左目图像上进行轮廓比对,如
方法三:首先获得雷达与双目相机视角一的配准外参TLC1,保持雷达固定,移动双目相机,转换视角,获得配准外参TLC2,则两不同视角下的点云转换外参为TC2C1=TLC1·
图 7. 双目相机两个不同视角的点云融合。(a)(b)两个视角的左视图;(c)点云融合结果
Fig. 7. Point cloud fusion of two different perspectives of binocular camera. (a)(b) Left view of two perspectives; (c) result of the point cloud fusion
通过融合结果
由以上三种方法的验证结果可以看出,利用本文方法可以很准确地获得激光雷达与双目相机的配准参数。
4.2 结果比较
为了体现本文方法的优越性,将其与文献[ 4]中特征点匹配求解的方法进行比较。本文方法与文献[ 4]中方法属于同种类型的方法,即都为通过提取特征点来进行匹配参数求解。本文与文献[ 4]中方法的主要区别有两点:一是特征点提取方式不同,二是配准参数求解方法不同。文献[ 4]中采取拟合圆、提取圆心点的方式,并通过 ICP(Iterative closest points)的方法求解配准参数。由于本文是按照一定顺序提取角点的(如逆时针提取边缘,并依次求取角点),所以提取出的两组点匹配关系已知,可以直接采用Kabsch算法进行配准参数求解,从而避免使用ICP算法陷入局部最优解的可能。
本文将采用两种方式进行比较:一是理论分析比较,二是结果图直观比较。
理论分析:首先,文献[
4]中采用带有4个圆形孔洞的大矩形平面木板,且该方法是直接在XY平面上对木板平面进行提取,并进而拟合出圆心,意味着传感器坐标系的Z轴必须与木板垂直,较容易引入不必要的误差。另外,文献[
4]中方法拟合圆心时所用的点太少,容易产生较大误差,如
图 8. 文献[ 4]中方法的融合结果图。(a)边缘点提取结果图;(b)圆心提取结果图
Fig. 8. Fusion results of method in Ref. [4]. (a) Result of edge point extraction; (b) result of circle center extraction
结果展示:文献[
4]中方法的融合结果如
图 9. 文献[ 4]中方法融合结果图。(a)结果一;(b)结果二
Fig. 9. Fusion results of method in Ref. [4]. (a) Result 1; (b) another result
5 结论
本文提出了一种基于3D-3D特征点的激光雷达与双目相机的外参配准方法。在已知双目相机内参的情况下,通过SGBM方法恢复视差图并获得点云,利用边缘提取算法提取木板边缘,拟合边缘求取木板角点,获得了双目相机坐标系下的3D特征点;对于激光雷达,映射其点云到左目图像上,选取木板边缘对应的点云,拟合直线求取木板角点。最后利用Kabsch算法对两种途径提取的配对特征点云进行配准,准确求解激光雷达与双目相机之间的配准参数,实验装置简单,方便搭建。
[1] Zhou LP, Li ZM, KaessM. Automatic extrinsic calibration of a camera and a 3D LiDAR using line and plane correspondences[C]∥2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), October 1-5, 2018, Madrid, Spain. New York: IEEE, 2018: 5562- 5569.
[2] GeigerA, MoosmannF, CarO, et al. Automatic camera and range sensor calibration using a single shot[C]∥2012 IEEE International Conference on Robotics and Automation, May 14-18, 2012, St Paul, MN, USA. New York: IEEE, 2012: 3936- 3943.
[3] GuindelC, BeltránJ, MartínD, et al. Automatic extrinsic calibration for lidar-stereo vehicle sensor setups[C]∥2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), October 16-19, 2017, Yokohama, Japan. New York: IEEE, 2017: 17631999.
[4] Vel'asM, ŠpanělM, MaternaZ, et al. Calibration of RGB camera with velodyne LiDAR[C]∥22nd International Conference in Central Europeon Computer Graphics, Visualization and Computer Vision, November 10, 2014. [S.l.: s.n.], 2014: 135- 144.
[5] CastorenaJ, Kamilov US, Boufounos PT. Autocalibration of lidar and optical cameras via edge alignment[C]∥2016 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP), March 20-25, 2016, Shanghai, China. New York: IEEE, 2016: 2862- 2866.
[6] 夏鹏飞, 尹慧琳, 何艳侠. 基于最大互信息的激光雷达与相机的配准[J]. 仪器仪表学报, 2018, 39(1): 34-41.
Xia P F, Yin H L, He Y X. Calibration of Lidar and camera based on maximum mutual information[J]. Chinese Journal of Scientific Instrument, 2018, 39(1): 34-41.
[7] TaylorZ, NietoJ. Automatic calibration of lidar and camera images using normalized mutual information[C]∥2013 IEEE International Conference on Robotics and Automation, ICRA 2013, May 6-10, 2013, Karlsruhe, Germany. New York: IEEE, 2013.
[8] PandeyG, McBride J R, Savarese S, et al. Automatic targetless extrinsic calibration of a 3D lidar and camera by maximizing mutual information[C]∥Twenty-Sixth AAAI Conference on Artificial Intelligence, July 22-26, 2012, Toronto, Ontario, Canada. California: AAAI, 2012: 2053- 2059.
[9] LevinsonJ, ThrunS. Automatic online calibration of cameras and lasers[C]∥Robotics: Science and Systems, Jun 24-28, 2013, Berlin, Germany. Cambridge: MIT Press, 2013.
[10] JohnV, LongQ, LiuZ, et al. Automatic calibration and registration of lidar and stereo camera without calibration objects[C]∥2015 IEEE International Conference on Vehicular Electronics and Safety (ICVES), November 5-7, 2015, Yokohama, Japan. New York: IEEE, 2015: 231- 237.
[11] BokY, Choi DG, Kweon IS. Generalized laser three-point algorithm for motion estimation of camera-laser fusion system[C]∥2013 IEEE International Conference on Robotics and Automation, May 6-10, 2013, Karlsruhe, Germany. New York: IEEE, 2013: 2880- 2887.
[12] Hirschmüller H. Stereo processing by semiglobal matching and mutual information[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2008, 30(2): 328-341.
[13] Fischler M A, Bolles R C. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981, 24(6): 381-395.
[14] Sorkine-Hornung O, Rabinovich M. Least-squares rigid motion using SVD[J]. Computing, 2017, 1(1): 1-5.
[15] 王岱良, 李玉. 基于旋转差值核估计的激光雷达点云建筑物边缘提取[J]. 中国激光, 2019, 46(1): 0104005.
[16] 苗松, 王建军, 李云龙, 等. 基于建筑物激光点云边缘线自动提取提高DSM精度[J]. 激光与光电子学进展, 2018, 55(1): 012803.
[17] 陈贵宾, 高振海, 何磊. 车载三维激光雷达外参数的分步自动标定算法[J]. 中国激光, 2017, 44(10): 1010004.
陈少杰, 朱振才, 张永合, 郭明, 支帅. 基于3D特征点的激光雷达与立体视觉配准方法[J]. 激光与光电子学进展, 2020, 57(3): 030102. Shaojie Chen, Zhencai Zhu, Yonghe Zhang, Ming Guo, Shuai Zhi. Extrinsic Calibration for Lidar and Stereo Vision Using 3D Feature Points[J]. Laser & Optoelectronics Progress, 2020, 57(3): 030102.