CN112017219B - 一种激光点云配准方法 - Google Patents

Abstract

本发明提供了一种激光点云配准方法，通过对两帧待配准的源三维点云和目标三维点云分别进行降采样并分割出非地面点云后，提取对应源三维点云和目标三维点云的局部特征的源局部特征和目标局部特征并进行特征匹配，进而基于匹配结果估计所述源三维点云相对于所述目标三维点云的位姿变换，得到源三维点云和目标三维点云之间的位姿变换矩阵完成对源三维点云和目标三维点云的配准。基于本发明提供的激光点云配准方法，通过对源三维点云和目标三维点云进行降采样，可以在减小计算压力的同时保留原始点云的信息，提高后续特征的稳定性。并且，基于非地面点云的关键点提取源三维点云和目标三维点云的局部特征，可以有效提升两帧三维点云的配准精度。

Description

一种激光点云配准方法

技术领域

本发明涉及自动驾驶技术领域，特别是涉及一种激光点云配准方法。

背景技术

激光雷达作为实现全自主无人驾驶不可或缺的传感器，具有探测范围广、对季节光照变化不敏感等特点。目前大多数无人驾驶汽车会在车顶安装一个360度扫描的主激光雷达，用于获取车体周围的环境信息。主激光雷达采集的三维点云可用于构建高精度地图、对车体进行定位等任务。这些任务都需要对不同时刻采集的三维点云进行配准，从而获取两幅三维点云之间的刚体变换关系。

当车体运动过快，或者车体重新回到之前走过的地方时，需要配准的两幅三维点云之间的相对位置通常变化很大，基于迭代最近邻的点云配准算法往往会失效，而利用三维局部特征匹配可以很好的处理这种情形。

现有的三维局部特征描述方法大多是针对一些室内小目标的重建、识别等任务设计的，这些任务中采用的三维传感器获取的三维点云数据分布比较均匀，精度较高。而激光雷达采集的三维点云，由于探测范围广，其点密度会根据场景的结构、目标的远近不同而分布不均，对于像树木、灌木丛这类物体，其点云的精度较差。传统的三维局部特征描述方法大多会利用每个点的法向量信息，而法向量的估计不仅耗时而且容易受到噪声以及分辨率不均匀的影响，所以最终得到的特征描述符也会对这些因素敏感。此外，上述特征描述方法大多只编码了局部形状信息，没有考虑点的其他信息。因此，针对激光三维点云，如何设计一种计算高效、区分性好、鲁棒性强的局部特征是亟待解决的问题。

发明内容

本发明提供了一种激光点云配准方法以克服上述问题或者至少部分地解决上述问题。

根据本发明的一个方面，提供了一种激光点云配准方法，包括：

获取两帧待配准的激光三维点云，分别记为源三维点云和目标三维点云；

对所述源三维点云和目标三维点云分别进行降采样并分割出地面点云和非地面点云，以基于各自地面点云的法向量和非地面点云中的关键点分别获取所述源三维点云和目标三维点云的局部特征，记为源局部特征和目标局部特征；

对所述源局部特征和目标局部特征进行特征匹配；

根据特征匹配结果估计所述源三维点云相对于所述目标三维点云的位姿变换，得到所述源三维点云和目标三维点云之间的位姿变换矩阵。

可选地，对于所述源三维点云和目标三维点云中的任意一组三维点云，均执行以下获取三维点云的局部特征的步骤，以分别得到所述源三维点云和目标三维点云的源局部特征和目标局部特征：

对所述三维点云进行降采样，得到所述三维点云的降采样三维点云；

从所述降采样三维点云中分割出地面点云和非地面点云；

估算所述地面点云的法向量；

提取所述非地面点云的关键点，以所述法向量为主轴建立圆柱坐标系下的水平投影距离和垂直投影分量，基于所述水平投影距离和垂直投影分量计算每个关键点的二维矩阵；

计算每个所述关键点的反射率直方图向量；

对所述二维矩阵和反射率直方图向量进行归一化，基于归一化后的二维矩阵和反射率直方图向量得到所述三维点云的局部特征。

可选地，对所述三维点云进行降采样，得到所述三维点云的降采样三维点云，包括：

获取所述三维点云中的边界坐标点；其中，所述边界坐标点包括：以激光雷达发射点为原点所建立的三维空间坐标系中，处于各维度的边界的坐标点；

基于所述边界坐标点构建可将所述三维点云包含在内的长方体空间；

将所述长方体空间离散为多个大小相同的立方体；

对于每个所述立方体，获取所述三维点云中位于所述立方体的空间内的至少一个第一选取点，并计算所有所述第一选取点的几何重心，将该立方体的空间中与所述几何重心最近的第一选取点作为降采样后的点；

将得到的每个立方体的降采样后的点保存到新的三维点云中，得到所述三维点云的降采样三维点云。

可选地，从所述降采样三维点云中分割出地面点云和非地面点云，包括：

从所述降采样三维点云中任取多组预设数量的不共线的第二选取点；

分别基于每组所述预设数量的第二选取点确定多个平面；

对于任意一组所述不共线的第二选取点，计算所述降采样三维点云中的每个第二选取点到所述平面的距离，并统计距离在设定距离范围内的第二选取点的数量，将满足所述距离约束的第二选取点作为该组第二选取点中的内点；

在所述多个平面中选取内点数最多的平面作为地面，将所述地面的内点构成的点云作为地面点云；

识别所述三维点云中除所述地面点云之外的第三选取点，并以所述第三选取点构建非地面点云。

可选地，估算所述地面点云的法向量，包括：

计算如下协方差矩阵M，

其中，|G|表示所述地面点云中包括的三维点的个数，q表示所述地面点云G中的一个三维点，

表示所述三维点云中包括的三维点的几何重心；

对所述协方差矩阵M进行特征值分解得到多个特征值，将所述多个特征值中最小的特征值对应的特征向量作为所述地面点云的法向量。

可选地，提取所述非地面点云的关键点，包括：

利用关键点检测算法从所述非地面点云中提取多个关键点。

可选地，以所述法向量为主轴建立圆柱坐标系下的水平投影距离和垂直投影分量，基于所述水平投影距离和垂直投影分量计算每个关键点的二维矩阵，包括：

对于任意一个关键点，以该关键点为球心，在所述三维点云中搜索所有与之距离不超过设定距离的第四选取点；

创建第一设定大小的预设矩阵；

对于每个所述第四选取点，计算所述第四选取点在以每个关键点为原心、所述法向量为主轴建立的圆柱坐标系下的水平投影距离和垂直投影分量；

分别计算所述水平投影距离和垂直投影分量在所述预设矩阵中的下标；其中，所述水平投影距离和垂直投影分量在所述预设矩阵中的下标分别作为所述预设矩阵的行和列；

基于所述水平投影距离和垂直投影分量在所述预设矩阵中的下标利用双线性插值在所述预设矩阵的矩阵坐标处累加权重值，以得到所述关键点的二维矩阵。

可选地，计算每个所述关键点的反射率直方图向量，包括：

创建第二设定大小的预设直方图向量；

获取每个所述关键点的反射率，基于每个所述关键点的反射率所述预设直方图向量的下标，以确定所述预设直方图向量的各坐标点，生成反射率直方图向量。

可选地，对所述二维矩阵和反射率直方图向量进行归一化，包括：

将所述二维矩阵按行依序变换成一维行向量并除以第四选取点的总数进行归一化得到形状特征向量；

以所述反射率直方图向量依序形成一维行向量除以第四选取点的总数进行归一化得到反射率特征向量。

可选地，基于归一化后的二维矩阵和反射率直方图向量得到所述三维点云的局部特征，包括：

将所述形状特征向量和所述反射率特征向量拼接得到任一关键点的三维局部特征向量；

将所述降采样三维点云中的所有关键点的三维局部特征向量汇总作为所述三维点云的局部特征。

本发明提供了一种高效的激光点云配准方法，通过对两帧待配准的源三维点云和目标三维点云分别进行降采样并分割出地面点云后，提取对应源三维点云和目标三维点云的局部特征的源局部特征和目标局部特征并进行特征匹配，进而基于匹配结果估计所述源三维点云相对于所述目标三维点云的位姿变换，得到源三维点云和目标三维点云之间的位姿变换矩阵完成对源三维点云和目标三维点云的配准。

基于本发明提供的激光点云配准方法，通过对源三维点云和目标三维点云进行降采样，可以在减小计算压力的同时保留原始点云的信息，提高后续特征的稳定性。并且，通过对降采样后的点云所分割出的非地面点云的关键点提取源三维点云和目标三维点云的局部特征，可以增强局部特征生成的效率以及特征的鲁棒性，进而有效提升源三维点云和目标三维点云的配准精度。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。

附图说明

通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：

图1是根据本发明实施例的激光点云配准方法流程示意图；

图2是根据本发明实施例的获取三维点云局部特征的获取方法；

图3是根据本发明实施例的三维点云示意图；

图4是根据图2所示三维点云的降采样三维点云示意图；

图5是根据图4所示降采样三维点云的地面点云示意图；

图6是根据图4所示降采样三维点云的非地面点云示意图；

图7是根据非三维点云中任意一个关键点为球心建立的圆柱的俯视图；

图8是根据本发明实施例的二维矩阵示意图；

图9是根据本发明实施例的反射率直方图示意图；

图10a是根据本发明实施例的源三维点云和目标三维点云配准前的示意图；

图11a是根据本发明实施例的源三维点云和目标三维点云配准过程的示意图；

图12a是根据本发明实施例的源三维点云和目标三维点云配准后的示意图。

具体实施方式

下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。

图1是根据本发明实施例的激光点云配准方法流程示意图，参见图1可知，本发明实施例提供的激光点云配准方法可以包括：

步骤S102，获取两帧待配准的激光三维点云，分别记为源三维点云和目标三维点云；

步骤S104，对源三维点云和目标三维点云分别进行降采样并分割出地面点云和非地面点云，以基于各自地面点云的法向量和非地面点云中的关键点分别获取源三维点云和目标三维点云的局部特征，记为源局部特征和目标局部特征；

步骤S106，对源局部特征和目标局部特征进行特征匹配；

步骤S108，根据特征匹配结果估计源三维点云相对于目标三维点云的位姿变换，得到源三维点云和目标三维点云之间的位姿变换矩阵。

本发明实施例提供了一种高效的激光点云配准方法，通过对两帧待配准的源三维点云和目标三维点云分别进行降采样并分割出非地面点云后，提取对应源三维点云和目标三维点云的局部特征的源局部特征和目标局部特征并进行特征匹配，进而基于匹配结果估计源三维点云相对于目标三维点云的位姿变换，得到源三维点云和目标三维点云之间的位姿变换矩阵完成对源三维点云和目标三维点云的配准。基于本发明实施例提供的激光点云配准方法，通过对源三维点云和目标三维点云进行降采样，可以在减小计算压力的同时保留原始点云的信息，提高后续特征的稳定性。并且，通过对降采样后的点云所分割出的非地面点云的关键点分别提取源三维点云和目标三维点云的局部特征，可以增强局部特征生成的效率以及特征的鲁棒性，进而对不同时刻的源三维点云和目标三维点云完成高效率的配准，获取两幅三维点云之间的刚体变换关系，以便于对车辆进行准确定位，或是构建高精度地图等等。

本实施例中的源三维点云和目标三维点云可通过激光雷达进行获取，激光雷达可以安装在测绘采集车辆或是其他车辆的车顶上，该激光雷达通过预设的扫描频率(如每隔0.1秒或是其他间隔时间扫描一次)扫描车辆周围的环境参数，以获取各个时刻的各帧三维点云。

在本实施例中，对源三维点云和目标三维点云的配准过程中重点在于对源三维点云和目标三维点各自的局部特征的获取。在本发明一可选实施例中，还提供了一种获取三维点云局部特征的获取方法，对于源三维点云和目标三维点云中的任意一组三维点云，均可通过该方法分别得到源三维点云和目标三维点云的源局部特征和目标局部特征。如图2所示，本发明实施例提供的三维点云局部特征的获取方法可以包括以下步骤S202～S210。

步骤S202，对三维点云进行降采样，得到三维点云的降采样三维点云。实际应用中，无论是源三维点云还是目标三维点云，所包括的点数量比较大并且比较密集，因此，在提取三维点云的局部特征之前，可以先对三维点云进行降采样。在本发明一可选实施例中，对三维点云进行降采样具体可以包括：

S1-1，获取三维点云中的边界坐标点；其中，边界坐标点包括：以激光雷达发射点为原点所建立的三维空间坐标系中，处于各维度的边界的坐标点；

S1-2，基于边界坐标点构建可将三维点云包含在内的长方体空间；

S1-3，将长方体空间离散为多个大小相同的立方体；

S1-4，对于每个立方体，获取三维点云中位于立方体的空间内的至少一个第一选取点，并计算所有第一选取点的几何重心，将该立方体的空间中与几何重心最近的第一选取点作为降采样后的点；

S1-5，将得到的每个立方体的降采样后的点保存到新的三维点云中，得到三维点云的降采样三维点云。

也就是说，假设任一三维点云L，该三维点云L中的每个三维点均包括一个以激光雷达发射点为原心的三维坐标(x,y,z)和一个反射率值γ，反射率值γ的取值范围可以为0到255之间的整数值。对该三维点云L进行降采样时，首先，可以找出三维点云L中的6个边界坐标，即x_min,x_max,y_min,y_max,z_min,z_max。由这6个边界坐标可以得到一个将三维点云L包含在内的长方体空间V。随后，将长方体空间V离散为若干个大小相同的小立方体。假设立方体的边长为l，则可以得到(x_max-x_min)(y_max-y_min)(z_max-z_min)/l³个小立方体。

对于每个小立方体空间，找出三维点云L中位于该小立方体内的点记为第一选取点，并计算第一选取点的几何重心。实际应用中，为了不改变原始激光点的位置，将该立方体空间中与几何重心最近的点作为降采样后的点。最终，将得到的每个小立方体的降采样后的点保存到新的三维点云中，得到三维点云的降采样三维点云记为C。

对于源三维点云和目标三维点云分别以上记载的降采样过程进行降采样，得到降采样源三维点云C₁和降采样目标三维点云C₂。图3是根据本发明实施例的源三维点云示意图，图4是对图3所示源三维点云进行降采样后的降采样源三维点云示意图。

步骤S204，从降采样三维点云中分割出地面点云和非地面点云。

在本发明一可选实施例中，从降采样三维点云中分割出地面点和非地面点云时，可以通过以下方式：

S2-1，从降采样三维点云中任取多组预设数量的不共线的第二选取点；

S2-2，分别基于每组预设数量的第二选取点确定多个平面；

S2-3，对于任意一组不共线的第二选取点，计算降采样三维点云中的每个第二选取点到平面的距离，并统计距离在设定距离范围内的第二选取点的数量，将满足距离约束的第二选取点作为该组第二选取点中的内点；

S2-4，在多个平面中选取内点数最多的平面作为地面，将地面的内点构成的点云作为地面点云；

S2-5，识别降采样三维点云中除地面点云之外的第三选取点，并以第三选取点构建非地面点云。

在本实施例中，可利用随机采样一致性算法从降采样源三维点云C₁和降采样目标三维点云C₂中分别分割出地面点云。下面以降采样源三维点云C₁进行分割地面点云为例进行详细说明。

(a)从降采样源三维点云C₁中任取三个(或其他数量个)不共线的点，由这三个点可以确定一个平面，为了防止提取到墙体等垂直结构的平面，要求该平面的法向量与向量[0,0,1]^T的夹角不能超过45°，如果超过，则再次选取三个不共线的点。

其中，向量[0,0,1]^T是设定的一个向量，该向量[0,0,1]^T与地面的夹角是45°，这里默认激光雷达水平安装的，即激光雷达坐标系的z轴朝上(沿水平面垂直向上)，x轴和y轴与水平面平行且二者呈90°。因此，该向量与地面的夹角是45°，如果激光雷达是其他安装角度，该向量[0,0,1]^T的值可以根据情况进行选取，本发明不做限定。

(b)计算C₁中每个点到该平面的距离，并统计距离在0.1米内的点的数量，满足这个距离约束的点称为内点；

(c)重复步骤(a)～(b)若干次，得到内点数最多的平面作为地面，而该地面所包括的内点构成的点云即为地面点云，记为G₁，如图5所示。

基于上述步骤(a)～(c)，可以分别得到降采样源三维点云C₁和降采样目标三维点云C₂对应的地面点云G₁和G₂。

进一步地，从降采样三维点云中分割出非地面点云时，可以先识别三维点云中除地面点云之外的第三选取点，并以第三选取点构建非地面点云。

以从一帧降采样三维点云C₁中提取三维点云的关键点为例，记三维点云C₁中所有的非地面点(即C₁中除了地面点云G₁之外的那些点云)构成的三维点云为F₁，如图6所示。

步骤S206，估算地面点云的法向量。

在本发明一可选实施例中，估算地面点云的法向量时，可以先计算如下协方差矩阵M，并对协方差矩阵M进行特征值分解得到多个特征值，将多个特征值中最小的特征值对应的特征向量作为地面点云的法向量n。

协方差矩阵M如下：

其中，|G|表示集合的势，即包含元素的个数。|G|包含的元素个数指其中总共有多少个点，在本实施例中表示地面点云中包括的三维点的个数，q表示地面点云G中的一个三维点，

表示三维点云中包括的三维点的几何重心，即平均值。得到协方差矩阵后，对其进行特征值分解，即得到地面点云的法向量。

为了估算地面点云G₁的法向量n₁∈R³，首先计算如下协方差矩阵：

|G₁|表示表示地面点云G₁中包括的三维点的个数。

举例来说，上述协方差矩阵M₁是一个3×3的矩阵，特征值有3个(n×n的矩阵，特征值有n个)，选取其中最小的那个特征值对应的特征向量即为G₁的法向量n₁∈R³。

设最小特征值对应的特征向量为v，由于-v也是与之对应的特征向量，为了消除这种二义性，从上述v和-v两个特征向量中选取与向量[0,0,1]^T夹角为锐角的那个特征向量作为地面的法向量，记为n₁。同样地，采用相同的方式可得到地面点云G₂的法向量n₂。

步骤S208，提取非地面点云的关键点，以法向量为主轴建立圆柱坐标系下的水平投影距离和垂直投影分量，基于水平投影距离和垂直投影分量计算每个关键点的二维矩阵。

可选地，可以利用关键点检测算法(如Harris3D检测算法)从F₁中提取若干关键点p。

进一步地，对于任意一个关键点，在计算该关键点的二维矩阵时，可以通过如下方式：

S3-1，该关键点为球心，在三维点云中搜索所有与之距离不超过设定距离的第四选取点；

S3-4，创建第一设定大小的预设矩阵；

S3-5，对于每个第四选取点，计算第四选取点在以每个关键点为原心、法向量为主轴建立的圆柱坐标系下的水平投影距离和垂直投影分量；

S3-6，分别计算水平投影距离和垂直投影分量在预设矩阵中的下标；其中，水平投影距离和垂直投影分量在预设矩阵中的下标分别作为预设矩阵的行和列；

S3-7，基于水平投影距离和垂直投影分量在预设矩阵中的下标利用双线性插值在预设矩阵的矩阵坐标处累加权重值，以得到关键点的二维矩阵。

以上述实施例提及的非三维点云为F₁中的任意一个关键点p举例来讲，如图7所示，以该点为球心，在降采样三维点云C₁中搜索所有与之距离不超过R'的点，记为Q₁(即上述实施例中的第四选取点)。为了加速搜索过程，利用Kd-tree存储三维点云，可以达到对数复杂度的搜索效率。

创建一个大小为N_β×N_α的二维矩阵m，如图8所示，二维矩阵m中的每个元素都初始化为0。

对于Q₁中的每一个点q，计算它在以每个关键点p为原心、法向量n₁为主轴建立的圆柱坐标系下的水平投影距离α和垂直投影分量β(参见图7)，α和β的计算公式如下：

α＝||d-d·n₁||₂ 公式(3)

β＝d·n₁+R' 公式(4)

其中，d＝q-p

由公式(3)和(4)可知，α和β的取值范围分别为[0,R']以及[0,2R']，上述R'是搜索邻域点的半径(radius)，该参数可根据不同的需求进行设置。

分别计算α和β在二维矩阵m中的下标

其中，

表示对x向下取整，上述公式中的N_β、N_α两个参数都是整数值，分别表示二维矩阵m的行数和列数。

随后，计算余数a＝βN_β/(2R')-i，b＝αN_α/R'-j，利用双线性插值给二维矩阵m相应的位置累加一个权重值。图8为本发明实施例的二维矩阵m示意图，其中，i表示第i行，j表示第j列，参见图8可知，二维矩阵中各元素的值计算公式可以为：

m'(i,j)＝m(i,j)+w₁ 公式(5)

m'(i+1,j)＝m(i+1,j)+w₂ 公式(6)

m'(i,j+1)＝m(i,j+1)+w₃ 公式(7)

m'(i+1,j+1)＝m(i+1,j+1)+w₄ 公式(8)

其中，w₁＝(1-a)(1-b)，w₂＝a(1-b)，w₃＝(1-a)b，w₄＝ab

也即，

m'(i,j)＝m(i,j)+(1-a)(1-b) 公式(9)

m'(i+1,j)＝m(i+1,j)+a(1-b) 公式(10)

m'(i,j+1)＝m(i,j+1)+(1-a)b 公式(11)

m'(i+1,j+1)＝m(i+1,j+1)+ab 公式(12)

在图8所示二维矩阵中，w₁＝(1-a)(1-b)，w₂＝a(1-b)，w₃＝(1-a)b，w₄＝ab，各数值前的“+”表示累加。

由于二维矩阵m的大小为N_β×N_α，为了防止下标越界，当下标i＝N_β-1或j＝N_α-1时，则直接在m(i,j)的位置加1即可。

步骤S210，计算每个关键点的反射率直方图向量。具体地，首先，创建第二设定大小的预设直方图向量；其次，获取每个关键点的反射率，基于每个关键点的反射率预设直方图向量的下标，以确定预设直方图向量的各坐标点，生成反射率直方图向量。

举例来讲，可以先创建一个大小为1×N_b的反射率直方图向量h，如图9所示，反射率直方图向量h中的每个元素都初始化为0。其中，N_b的值可以根据不同需求进行设置，本发明不做限定。

假设点q的反射率为γ，则该点对应在反射率直方图向量h中的下标的计算公式为：

同时，在h(k)的位置加1，由此生成反射率直方图向量h。

步骤S212，对二维矩阵和反射率直方图向量进行归一化，基于归一化后的二维矩阵和反射率直方图向量得到三维点云的局部特征。

对于二维矩阵进行归一化时，可以将二维矩阵按行依序变换成一维行向量并除以第四选取点的总数进行归一化得到形状特征向量。对于反射率直方图向量进行归一化时，可以以反射率直方图向量依序形成一维行向量除以第四选取点的总数进行归一化得到反射率特征向量。

最后，基于归一化后的二维矩阵和反射率直方图向量得到三维点云的局部特征。也就是说可以将形状特征向量和反射率特征向量拼接得到任一关键点的三维局部特征向量；将降采样三维点云中的所有关键点的三维局部特征向量汇总作为三维点云的局部特征。

也就是说，当Q中所有关键点的二维矩阵均计算完成后，将各关键点的二维矩阵m拉成一维行向量并除以Q中所包含关键点的总数进行归一化进而得到形状特征向量。

其中m_i表示矩阵m的第i行。

同理，对反射率直方图向量h进行同样的归一化操作，得到反射率特征向量。

r＝[h(0),h(1),...,h(N_b-1)]/|Q|. 公式(14)

将形状特征向量与反射率特征向量串联起来即为关键点p的三维局部特征向量，表示如下：

通过上述实施例各步骤，可得到三维关键点p的特征向量，得到其对应的三维局部特征向量f(p)。

需要说明的是，这里只是三维点云C₁中一个关键点的特征向量，通常待匹配的两幅三维点云中需要各自提取几百个三维关键点，每个关键点都需要根据上面的特征描述方法计算特征，最终点云C₁会得到N₁个特征，记为

点云C₂会得到N₂个特征，记为

然后对N₁和N₂个特征进行匹配，从而得到源三维点云和目标三维点云的对应关系。可选地，N₁和N₂的数量可优选为300。

最后，将上述得到的两帧三维点云的三维局部特征进行匹配，特征匹配策略可选最近邻匹配或者k近邻匹配。根据特征匹配结果估计粗匹配位姿变换，粗匹配位姿变换可采用随机采样一致性算法或者几何一致性分组算法实现。

上述得到的三维点云的特征描述为三维特征向量，是用3×3的旋转矩阵来表示的，通过旋转和平移变换成4×4的齐次变换矩阵，通过位姿变换可以对两幅点云进行对齐，得到两幅点云之间的位置关系，也即刚体变换关系。

在本发明可选实施例中，得到源三维点云和目标三维点云之间的位姿变换矩阵之后，还可以对其进行优化，如采用迭代最近邻算法(Iterative Closest Point,ICP)或者G-ICP(General-Iterative Closest Point)算法调优，从而提升源三维点云和目标三维点云的配准精度。

本发明提出的快速可靠的激光点云配准方法，在体素下采样的时候取原始点云中距离每个体素几何重心最近的点作为降采样的点可以保留原始点云的信息，提高后续特征的稳定性；利用地面点云的法向量作为每个关键点的参考轴，可以提高源局部特征和目标局部特征生成的效率以及特征的鲁棒性；从非地面点云中提取关键点可以提高关键点的区分性；融合形状信息和反射率信息可以增强特征的区分性，提高匹配准确率。最终的特征描述符的维度为N_αN_β+N_b，研究发现取N_α＝8,N_β＝8,N_b＝32时，有利于节省存储空间以及加速后续的特征匹配效率。

为了更好的展现本实施例中源三维点云和目标三维点云的效果，图10a示出了本发明实施例的源三维点云和目标三维点云配准前的示意图。图11a示出了本发明实施例的源三维点云和目标三维点云配准过程的示意图。图11a中的上层为源三维点云，下层为目标三维点云。图12a示出了本发明实施例的源三维点云和目标三维点云配准后的示意图。在图10a、图11a、和图12a所示的示意图中，两组三维点云是通过无人车在同一路口采集到的间隔时间为6分钟的点云数据。通过对比图10a和图12a可知，基于本发明实施例的提供的激光点云配准方法可以使得源三维点云和目标三维点云有较高的配准精度。

基于同一发明构思，本发明实施例还提供了一种计算机可读存储介质，其特征在于，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行如上述任意一个实施例所述的激光点云配准方法。

基于同一发明构思，本发明实施例还提供了一种电子设备，其特征在于，包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如上述任意一个实施例所述的激光点云配准方法。

所属领域的技术人员可以清楚地了解到，上述描述的系统具体工作过程，可以参考前述方法实施例中的对应过程，为简洁起见，在此不另赘述。

本领域普通技术人员可以理解：上述的方法如果以软件的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，其包括若干指令，用以使得一台计算设备(例如个人计算机，服务器，或者网络设备等)在运行所述指令时执行本发明各实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM)、随机存取存储器(RAM)，磁碟或者光盘等各种可以存储程序代码的介质。

或者，实现前述方法实施例的全部或部分步骤可以通过程序指令相关的硬件(诸如个人计算机，服务器，或者网络设备等的计算设备)来完成，所述程序指令可以存储于一计算机可读取存储介质中，当所述程序指令被计算设备的处理器执行时，所述计算设备执行本发明各实施例所述方法的全部或部分步骤。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：在本发明的精神和原则之内，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案脱离本发明的保护范围。

Claims (9)

1.一种激光点云配准方法，包括：

获取两帧待配准的激光三维点云，分别记为源三维点云和目标三维点云；

对所述源三维点云和目标三维点云分别进行降采样并分割出地面点云和非地面点云，以基于各自地面点云的法向量和非地面点云中的关键点分别获取所述源三维点云和目标三维点云的局部特征，记为源局部特征和目标局部特征；

对所述源局部特征和目标局部特征进行特征匹配；

根据特征匹配结果估计所述源三维点云相对于所述目标三维点云的位姿变换，得到所述源三维点云和目标三维点云之间的位姿变换矩阵；

其中，对于所述源三维点云和目标三维点云中的任意一组三维点云，均执行以下获取三维点云的局部特征的步骤，以分别得到所述源三维点云和目标三维点云的源局部特征和目标局部特征：

对所述三维点云进行降采样，得到所述三维点云的降采样三维点云；

从所述降采样三维点云中分割出地面点云和非地面点云；

估算所述地面点云的法向量；

提取所述非地面点云的关键点，以所述法向量为主轴建立圆柱坐标系下的水平投影距离和垂直投影分量，基于所述水平投影距离和垂直投影分量计算每个关键点的二维矩阵；

计算每个所述关键点的反射率直方图向量；

对所述二维矩阵和反射率直方图向量进行归一化，基于归一化后的二维矩阵和反射率直方图向量得到所述三维点云的局部特征；

其中，以所述法向量为主轴建立圆柱坐标系下的水平投影距离和垂直投影分量，包括：

对于任意一个关键点，以该关键点为球心，在所述三维点云中搜索所有与之距离不超过设定距离的第四选取点；

创建第一设定大小的预设矩阵；

对于每个所述第四选取点，计算所述第四选取点在以每个关键点为原心、所述法向量为主轴建立的圆柱坐标系下的水平投影距离和垂直投影分量。

2.根据权利要求1所述的方法，其中，对所述三维点云进行降采样，得到所述三维点云的降采样三维点云，包括：

获取所述三维点云中的边界坐标点；其中，所述边界坐标点包括：以激光雷达发射点为原点所建立的三维空间坐标系中，处于各维度的边界的坐标点；

基于所述边界坐标点构建可将所述三维点云包含在内的长方体空间；

将所述长方体空间离散为多个大小相同的立方体；

对于每个所述立方体，获取所述三维点云中位于所述立方体的空间内的至少一个第一选取点，并计算所有所述第一选取点的几何重心，将该立方体的空间中与所述几何重心最近的第一选取点作为降采样后的点；

将得到的每个立方体的降采样后的点保存到新的三维点云中，得到所述三维点云的降采样三维点云。

3.根据权利要求1所述的方法，其中，从所述降采样三维点云中分割出地面点云和非地面点云，包括：

从所述降采样三维点云中任取多组预设数量的不共线的第二选取点；

分别基于每组所述预设数量的第二选取点确定多个平面；

对于任意一组所述不共线的第二选取点，计算所述降采样三维点云中的每个第二选取点到所述平面的距离，并统计距离在设定距离范围内的第二选取点的数量，将满足距离约束的第二选取点作为该组第二选取点中的内点；

在所述多个平面中选取内点数最多的平面作为地面，将所述地面的内点构成的点云作为地面点云；

识别所述降采样三维点云中除所述地面点云之外的第三选取点，并以所述第三选取点构建非地面点云。

4.根据权利要求1所述的方法，其中，估算所述地面点云的法向量，包括：

计算如下协方差矩阵M，

其中，|G|表示所述地面点云中包括的三维点的个数，q表示所述地面点云G中的一个三维点，

表示所述三维点云中包括的三维点的几何重心；

对所述协方差矩阵M进行特征值分解得到多个特征值，将所述多个特征值中最小的特征值对应的特征向量作为所述地面点云的法向量。

5.根据权利要求1所述的方法，其特征在于，

提取所述非地面点云的关键点，包括：

利用关键点检测算法从所述非地面点云中提取多个关键点。

6.根据权利要求1所述的方法，其中，基于所述水平投影距离和垂直投影分量计算每个关键点的二维矩阵，包括：

分别计算所述水平投影距离和垂直投影分量在所述预设矩阵中的下标；其中，所述水平投影距离和垂直投影分量在所述预设矩阵中的下标分别作为所述预设矩阵的行和列；

基于所述水平投影距离和垂直投影分量在所述预设矩阵中的下标利用双线性插值在所述预设矩阵的矩阵坐标处累加权重值，以得到所述关键点的二维矩阵；

其中，分别计算所述水平投影距离和垂直投影分量在所述预设矩阵中的下标，包括：

按照

计算所述水平投影距离和垂直投影分量在所述预设矩阵中的下标；

其中，i是所述水平投影距离α在所述预设矩阵中的下标、j是所述垂直投影分量β在所述预设矩阵中的下标、R'是搜索邻域点的半径、N_β和N_α分别表示所述预设矩阵的行数和列数。

7.根据权利要求6所述的方法，其中，所述计算每个所述关键点的反射率直方图向量，包括：

创建第二设定大小的预设直方图向量；

获取每个所述关键点的反射率，基于每个所述关键点的反射率计算所述关键点在所述预设直方图向量的下标，以确定所述预设直方图向量的各坐标点，生成反射率直方图向量；

其中，基于每个所述关键点的反射率计算所述关键点在所述预设直方图向量的下标，包括：

按照

计算所述关键点在所述预设直方图向量的下标；

其中，k是所述关键点在所述预设直方图向量的下标、γ是所述关键点的反射率、反射率直方图向量对应大小为1×N_b。

8.根据权利要求7所述的方法，其中，对所述二维矩阵和反射率直方图向量进行归一化，包括：

将所述二维矩阵按行依序变换成一维行向量并除以第四选取点的总数进行归一化得到形状特征向量；

以所述反射率直方图向量依序形成一维行向量除以第四选取点的总数进行归一化得到反射率特征向量。

9.根据权利要求8所述的方法，其中，基于归一化后的二维矩阵和反射率直方图向量得到所述三维点云的局部特征，包括：

将所述形状特征向量和所述反射率特征向量拼接得到任一关键点的三维局部特征向量；

将所述降采样三维点云中的所有关键点的三维局部特征向量汇总作为所述三维点云的局部特征。

Images