本技术公开了一种基于3D特征点的激光雷达与立体视觉配准方法，包括：进行双目相机特征点提取，通过半全局块匹配方法获得视差图，通过相机内参计算出深度，并进而计算出点云，通过边缘提取算法提取对象边缘，拟合边缘求取对象角点，获得双目相机坐标系下的
3D特征点；进行激光雷达特征点提取，将雷达点云映射到双目相机的左目图像上，选取对
象边缘对应的点云，拟合直线求取对象角点，获得雷达坐标系下的3D特征点；以及求解激光雷达和双目相机的配准参数。此配准方法简单易行，可自动完成多次测量，相比于同类方法精度也有所提高。
技术要求
1.一种基于3D特征点的激光雷达与立体视觉配准方法，包括：
进行双目相机特征点提取，通过半全局块匹配方法获得视差图，通过相机内参计算出深度，并进而计算出点云，通过边缘提取算法提取对象边缘，拟合边缘求取对象角点，获得双目相机坐标系下的3D特征点；
进行激光雷达特征点提取，将雷达点云映射到双目相机的左目图像上，选取对象边缘对应的点云，拟合直线求取对象角点，获得雷达坐标系下的3D特征点；以及求解双目相机坐标系下的3D特征点和雷达坐标系下的3D特征点的配准参数。
2.如权利要求1所述的基于3D特征点的激光雷达与立体视觉配准方法，其特征在于，按照
逆时针的顺序，依次提取双面相机图像中的边缘，根据像素坐标提取对应空间点云，利用随机抽样一致性算法拟合点云的直线。
3.如权利要求2所述的基于3D特征点的激光雷达与立体视觉配准方法，其特征在于，当拟
合出的两条空间直线不相交时，选用直线间最短线段的中点作为待求的角点。
4.如权利要求1所述的基于3D特征点的激光雷达与立体视觉配准方法，其特征在于，通过Kabsch算法进行配准参数的求解。
5.如权利要求1所述的基于3D特征点的激光雷达与立体视觉配准方法，其特征在于，进行
激光雷达特征点提取还包括通过初始旋转矩阵R0将雷达点云旋转至与双目相机坐标轴方向大致相同。
6.如权利要求1所述的基于3D特征点的激光雷达与立体视觉配准方法，其特征在于，最终
激光雷达与双目的配准参数为旋转矩阵R与初始旋转矩阵R0的乘积，其中
式中：其中U为正交矩阵、V为正交矩阵，d＝sign(det(UVT))。
7.如权利要求1所述的基于3D特征点的激光雷达与立体视觉配准方法，其特征在于，求解
激光雷达和双目相机的配准参数包括多次估计旋转和平移参数。
8.如权利要求7所述的基于3D特征点的激光雷达与立体视觉配准方法，其特征在于，经过N次求解，对所有配准参数进行聚类去除异常值后求取均值作为最终的最优结果。
技术说明书
基于3D特征点的激光雷达与立体视觉配准方法技术领域
本技术涉及图像处理技术领域。具体而言，本技术涉及基于3D特征点的激光雷达与立体视觉配准方法。背景技术
近年来，无人车行业迅速发展，预计到2021年，无人车将进入市场，从此进入新的阶段。无人车系统主要分为三个部分：算法端、客户端、云端。其中算法端主要任务是：利用传感器获取原始数据并从中提取有用的信息以便了解周围环境状况，进而根据环境具体状况做出判断。无人驾驶中通常所使用的传感器包括：全球定位系统(GPS)、惯性测量单元(IMU)、激光雷达、摄像头等。GPS与IMU融合是无人车里程计的主要数据来源，激光雷达与摄像头融合是环境感知、目标检测的主要设备。激光雷达测距远、精度高，但信息量少，无法获得颜色信息，目标检测受限，需融合视觉信息，形成优势互补。摄像头又可分为单目相机、双目相机、深度相机(RGB-D)相机。单目相机具有尺度不确定性，有一定局限性。RGB-D相机分辨率低，且只适应于室内环境。双目相机不仅具有单目相机的全部优点，还可以通过视差感知深度信息。因此激光雷达与双目相机融合具有一定优势。
目前激光雷达与相机配准的方法主要分为两大类：一是通过提取点、线等特征集，然后通过雷达点云与图像特征之间的匹配，求解两者之间的配准参数。二是通过计算雷达点云与相机的图像之间的互信息损失函数，然后利用优化算法求解配准参数，但此类方法只适用于传感器位移及旋转较小时。还有一些通过将SFM算法用于参数配准，但其需要多个辅助摄像头，对硬件成本要求较高。技术内容
本技术提出的方法属于通过提取特征点匹配求解，可以通过简单的实验装置，快速有效的求得激光雷达与双目相机之间的配准参数，为激光雷达与立体视觉数据融合提供基础。
根据本技术的一个方面，提供一种一种基于3D特征点的激光雷达与立体视觉配准方法，包括：
进行双目相机特征点提取，通过半全局块匹配方法获得视差图，通过相机内参计算出深度，并进而计算出点云，通过边缘提取算法提取对象边缘，拟合边缘求取对象角点，获得双目相机坐标系下的3D特征点；
进行激光雷达特征点提取，将雷达点云映射到双目相机的左目图像上，选取对象边缘对应的点云，拟合直线求取对象角点，获得雷达坐标系下的3D特征点；以及
求解激光雷达和双目相机的配准参数。
在本技术的一个实施例中，按照逆时针的顺序，依次提取双面相机图像中的边缘，根据像素坐标提取对应空间点云，利用随机抽样一致性算法拟合点云的直线。
在本技术的一个实施例中，当拟合出的两条空间直线不相交时，选用直线间最短线段的中点作为待求的角点。
在本技术的一个实施例中，通过Kabsch算法进行配准参数的求解。
在本技术的一个实施例中，进行激光雷达特征点提取还包括通过初始旋转矩阵R0将雷达点云旋转至与双目相机坐标轴方向大致相同。
在本技术的一个实施例中，最终激光雷达与双目的配准参数为旋转矩阵R与初始旋转矩阵
R0的乘积，其中
式中：d＝sign(det(UVT))。
在本技术的一个实施例中，求解激光雷达和双目相机的配准参数包括多次估计旋转和平移参数。
在本技术的一个实施例中，经过N次求解，对所有配准参数进行聚类去除异常值后求取均值作为最终的最优结果。
通过本技术公开的方法能够获得准确配准参数，并且此配准方法简单易行，可自动完成多次测量，相比于同类方法精度也有所提高。附图说明
为了进一步阐明本技术的各实施例的以上和其它优点和特征，将参考附图来呈现本技术的各实施例的更具体的描述。可以理解，这些附图只描绘本技术的典型实施例，因此将不被认为是对其范围的。在附图中，为了清楚明了，相同或相应的部件将用相同或类似的标记表示。
图1示出根据本技术的一个实施例的基于3D特征点的激光雷达与立体视觉配准方法的流程图。
图2示出半全局块匹配算法的整体框架。
图3示出成像系统，(a)为视差与深度几何关系；(b)为像素坐标到空间点。
图4示出根据本技术的基于3D特征点的激光雷达与立体视觉配准的系统的示例场景图。图5示出边缘提取的示意图，其中(a)(b)是双目相机边缘提取结果的不同视角；(c)激光雷达点云框选图。
图6示出两个场景下融合的不同视角，其中(a)(b)示出场景一；(c)(d)示出场景二。图7示出雷达点云变换后映射到左目图像的示意图。具体实施方式
在以下的描述中，参考各实施例对本技术进行描述。然而，本领域的技术人员将认识到可在没有一个或多个特定细节的情况下或者与其它替换和/或附加方法、材料或组件一起实施各实施例。在其它情形中，未示出或未详细描述公知的结构、材料或操作以免使本技术的各实施例的诸方面晦涩。类似地，为了解释的目的，阐述了特定数量、材料和配置，以便提供对本技术的实施例的全面理解。然而，本技术可在没有特定细节的情况下实施。此外，应理解附图中示出的各实施例是说明性表示且不一定按比例绘制。
在本说明书中，对“一个实施例”或“该实施例”的引用意味着结合该实施例描述的特定特征、结构或特性被包括在本技术的至少一个实施例中。在本说明书各处中出现的短语“在一个实施例中”并不一定全部指代同一实施例。
本本技术的实施例主要涉及激光雷达与双目相机的配准。求解激光雷达与双目相机之间的配准参数，需要提取一组对应的雷达与相机各自坐标系下的3D点，本技术选取两块木板的八个角点作为待求解的点。双目相机与激光雷达的角点提取采用相似的处理流程，即都是通过提取木板边缘3D点并拟合对应的直线，求取直线交点。下面将结合实施例介绍该方法的具体操作过程。
图1示出根据本技术的一个实施例的基于3D特征点的激光雷达与立体视觉配准方法的流程图。
首先，在步骤110，进行双目相机特征点提取。双目立体视觉模拟人类视觉处理景物方式，融合两只眼睛获得的图像并观察它们之间的差别，从而可以感知深度。将空间中同一物理点在不同图像中的像素点匹配起来，建立特征间的对应关系，形成视差(Disparity)图。关于视差图计算，有很多种方法，其中最经典的就是半全局匹配(Semi-
GlobalMatching，SGM)，可以发现目前排名靠前的算法中，很多都是对SGM的改进，具
有最强的实用价值。综合考虑效果与计算速度，选取SGM的一种改进算法：半全局块匹配(Semi-GlobalBlock Matching，SGBM)，通过此方法可以获得较精确的视差图，其算法整体框架如图2所示。
在获得视差图的基础上，通过相机内参计算出深度，并进而计算出点云x、y、z坐标。具体转换原理如图3(a)所示，P1、P2为空间点P分别在左右像平面上的成像点，f为相机焦距
(左右相机一样)，OL、OR分别为左右相机的光心。b为基线，表示两光心之间的距离。XL、XR分别为两个成像点距离各自所在图像的左边缘距离，XL与XR的差值成为视差。
根据三角形相似原理可求得：
其中XL-XR为视差值。若左右目相机内参中cx不一致，深度值需修改为：
式中：doffs为两个相机主点在x方向上的差距：cx1-cx0。
求得的Z为深度值，也即使点云对应的Z轴坐标，继续求出X与Y轴坐标，即可获得点云空间坐标。如图3(b)所示根据空间几何关系可得：
式中：u0、v0、fx、fy为相机内参，u、v为待求点云在视差图上的像素坐标。
按照从左到右逆时针的顺序，依次提取图像中木板边缘，根据像素坐标可提取对应空间点云，利用随机抽样一致性算法(RANSAC)算法拟合点云的直线。由于存在误差，拟合出的两条空间直线可能不会相交，此时选用直线间最短线段的中点作为待求的角点。
在步骤120，进行激光雷达特征点提取。激光雷达特征点提取方法与双目相机类似。激光雷达默认坐标系Z轴朝上、X轴朝前，而双目相机坐标系Z轴朝前、X轴朝右，首先需要通过一个初始旋转矩阵R0将雷达点云旋转至与双目相机坐标轴方向大致相同，方便后续操作。
关于激光点云，可以通过Levinson J,Thrun S.Automatic Online Calibrationof Cameras and
Lasers[C]//Robotics:Science and Systems.2013,2中公开的方法找到深度不连续性。木板平面
中的每个点，被赋予一个幅度，表示相对于其邻居的深度差异，并设置一个差异阈值保留木板边缘上的点云。对于空间上点较难直接分割出边缘各自对应的点云，平面上的点相对容易分割。可以利用所设置的初始旋转矩阵R0与左目相机内参将过滤后的点云映射到相机左目像素平面上，此过程通过式(4)来计算完成。
从左到右逆时针依次框选木板的边缘，然后拟合直线，求取角点。其顺序与相机边缘拟合求解一致，目的是便于两者之间角点匹配。
接下来，在步骤130，求解激光雷达和双目相机的配准参数。在本技术的实施例中，可通过Kabsch算法进行配准参数的求解。
用M、N分别表示两组对应的点云，Mi、Ni分别表示M、N集合内的点。那么优化的目标为：
对t求偏导，并令等式为零，得
(7)代入优化目标函数(5)，并令X′＝RX，得
由(10)可知，求(5)最小值，相当于求Tr(YTX′)最大值。因为X′＝RX，所以
Tr(YTX′)＝Tr(YTRX)＝Tr(XYTR)， (11)
对XYT做奇异值分解(SVD)，XYT＝UDVT，其中U为正交矩阵、D为对角矩阵，V为正交矩阵，从而得到：
令Z＝VTRU，则有
因为R和U都是正交阵，所以Z也为正交阵，则有det(Z)＝±1令Zii＝1，则有
R必须是右手系下的旋转，因此(14)式修正为：
式中：d＝sign(det(UVT))。
由于最初对雷达点云做过一次旋转变换，所以此处求解得到的旋转矩阵R需乘上初始旋转变换才能得到最终激光雷达与双目的配准参数。
即使双目相机保持不动，在静止的封闭房间内，通过实时显示双目相机建立的点云，可以发现，点云并不是静止的，来自雷达的点云同样也是有一定的浮动范围。为了减少噪声影响，可以多次估计旋转和平移参数。
经过N次求解，对所有配准参数进行聚类去除异常值后求取均值作为最终的最优结果。对于旋转矩阵的均值求解不太容易，且由于四元数可以直接累加，因此可以先把旋转矩阵转换为四元数。四元数与旋转矩阵之间转换关系如下：设四元数q＝q0+q1i+q2j+q3k，对应的旋转矩阵R为
反之，假设R＝{mij}，i,j∈[1,2,3]，其对应四元数q为
由于事实上一个旋转矩阵对应的四元数表示不唯一，还有其他一些转换方式，且当q0接近0时，其余三分量会非常大，导致解不稳定。因此可以使用其他解法，此处不再赘述。
图4示出根据本技术的基于3D特征点的激光雷达与立体视觉配准的系统的示例场景图。如图4所示，其中，(a)木板设置图；(b)激光雷达与双目相机放置图，在本示例中，使用速腾聚创的RS-LiDAR-16激光雷达，相机机采用ZED双目，其主要由两个高分辨率镜头组成，可通过GPU加速计算。
配准对象为为两块矩形木板，提供8个角点，方便提取特征点对。为了避免周围物体的影响，通过将绳子固定在木板背面，倾斜放置，使木板竖直悬挂在空中。激光雷达与双目相机大概位于两块木板平面正前方2m左右，与木板中心大概处于同一水平面。这样做的目的是保证木板每条边都可以包含一定数量的雷达扫描线，以及木板位于双目相机的成像区域。另外，在开始矫正前，相机的内参为已知条件。
通过双目相机提取的木板边缘点云如图5(a)、(b)所示。激光雷达提取边缘点云映射到像素平面上，并通过鼠标框选依次提取边缘对应雷达点云的效果如图5(c)所示。
在本技术的实施例中，采用两种方法来查看融合结果：一是，直接通过联合标定的外参融合激光雷达与双目相机的各自产生的点云；二是，通过将雷达点云通过相机的内参与联合标定的外参将激光雷达点云映射到双目相机的左目图像中。
点云融合结果如图6所示，在两种不同场地上的结果。从整体上、以及地平面部分点云融合结果来看，从过本文提出的方法可以有效的对激光雷达与双目相机进行配准。为了体现效果，通过将木板上的雷达点云重投影到左目图像上进行轮廓比对，如图7所示。可以看到，轮廓吻合较好，说明配准参数比较准确。
由以上两种方法验证结果可以看出，利用本文提出的方法可以很准确的获得激光雷达与双目相机的配准参数。
尽管上文描述了本技术的各实施例，但是，应该理解，它们只是作为示例来呈现的，而不作为。对于相关领域的技术人员显而易见的是，可以对其做出各种组合、变型和改变而不背离本技术的精神和范围。因此，此处所公开的本技术的宽度和范围不应被上述所公开的示例性实施例所，而应当仅根据所附权利要求书及其等同替换来定义。
