改进的采样一致性点云配准算法

王月海，庄志鹏，邢 娜

(北方工业大学 信息学院，北京 100144)

0 引 言

点云配准是三维重建行业[1-4]的核心技术和理论研究基础，一直备受学者关注[5-8]。迭代最近点配准算法是目前运用最为广泛的点云配准算法，该算法经过在两组点云之间探寻距离间隔最近的点对，并迭代计算出点对间的最优旋转平移矩阵。比较经典的点云配准算法是Besl等提出的ICP点云配准算法，该算法配准精度比较高，但是算法要求待配准数据具有较好的初值，不然容易得到局部最优的配准结果，为此国内外学者纷纷提出许多富有创造性的改进算法[9-13]，文献[10]使用内部形态描述子算法来提取点云的特征，进而使用采样一致性配准算法实现点云的配准，但其并未对点云数据进行滤波，因此配准时间较长。文献[11]在点云配准中，利用中心重合法，使得目标点云Q与待配准点云P的中心重合，降低两组点云数据的平移偏差。对于旋转偏差小的点云，该方法能以较快的速度完成配准，对于旋转偏差过大的点云，配准精度就会降低。文献[12]提出基于曲率极值的配准算法，针对曲率变换明显的点云数据表现出较好的配准效果，但是对于含有大量噪声的点云数据，算法配准质量较差。文献[13]提出了一种将SAC-IA与ICP算法相融合的点云配准方法，该方法对点云数据的快速点特征直方图FPFH[14-17]特征进行特征点的表示，随后采用点云配准算法(sample consensus initial alignment，SAC-IA)进行特征点的匹配，由此得到点云之间的对应关系，最终计算出点云的初始变换，得到一个较好的配准结果，但该方法对于初始点云数据没有做优化处理，因此配准时间较长，原始点云中的噪点也会影响配准精度。本文针对原始点云数据做优化处理，与随机采样一致性算法[13]不一样，使用自适应体素网格滤波法，降低数据量级的同时，保留了点云数据的原始分布特征，在快速点特征直方图求解阶段引入距离的二次函数，降低远距离邻域点的权值，同时提高近距离邻域点的权值，本算法整体相较于传统算法有较大提升。

1 采样一致性算法

点云数据的初始配准结果对于整个算法的最终配准结果的精度影响很大，采样一致性初始配准算法，是目前初始配准当中精度较高的算法之一，该算法是基于快速点云特征直方图FPFH的配准模式。通过计算每站点云的点特征直方图来描述点云局部特征，并用这个特征作为标准，进行两站点云的配准。经过多次迭代，选取其中最符合整体特征匹配的变换矩阵。在SAC-IA算法执行之前，首先需要计算点云的局部几何特征描述符，算法基本流程为：

(1)选取采样点：从源点云P中选取m个点，且每个点之间需要能满足距离大于提前给定的最小距离阈值d，这是为了尽量保证所采集的点云之间处在不同的曲面上，使得点之间能具有不同的FPFH，减少内存消耗的同时增大采样点集合的信息熵。

(2)查找采样点的对应点：对于源点云P上的m个采样点在目标点云Q中寻找与该采样点具有类似特征的点集合，再随机的从这些类似点里选择一个点作为源点云P的采样点在目标点云Q中的对应点。

(3)计算变换矩阵：计算源点云P上的m个采样点与目标点云Q的对应点之间的旋转变换矩阵，然后分别计算经过该变换之后的距离误差和函数，对当前配准的精确情况进行判别。本算法使用的误差和函数是Huber

(1)

式中：ml是一个给定的误差阈值，取值0.001。li表示经过变换后第i组对应点之间的距离差值。配准过程中寻找可以使得距离误差和函数Huber值达到最小的旋转平移矩阵，将其作为最终的配准变换矩阵，最终求得配准结果。

2 自适应采样一致性算法

2.1 自适应体素网格滤波法

由于通过三维激光雷达采集到的三维点云数据量极其庞大，平均每一帧点云数据的数量能达到上万个点且伴随着大量噪声，预处理阶段的三维点云数据与点云配准耗费的时长成正比，而噪声的存在会降低配准的准确度，因此在对点云配准处理前，需要先进行数据预处理。在对点云数据进行预处理时，应当尽可能地保持原始点云曲面法向量以及点云密度分布的特点。若使用随机采样法，采样点云的密度分布与原点云相差较大，而点云的曲面法向量等轮廓信息也难以得到完整保留，使用这种算法进行特征提取，极易得到特征扭曲的FPFH，不利于后续配准，因此需要一种既能减少点云数量，又能保留点云分布特征的滤波算法。

自适应体素网格滤波算法的基本原理是：通过边长大小为L的立方体网格，将原始输入点云等间距地分成N份，计算每个体素网格内点云的重心，将该重心替代网格内的点。具体计算方法：

(1)首先确定体素网格的边长L，因为每个体素网格将保留一个重心点，所以立方体大小直接影响着采样后点云分辨率的大小。L越大，采样后的点云分辨率越低，与之相反，分辨率则越高。采样立方体的个数为A×B×C， 计算方法如下

(2)

式中：xmax,xmin,ymax,ymin,zmax,zmin分别为X、Y、Z这3个坐标轴上点云坐标值相对应的最大值和最小值。

(2)将所有求得的重心代替当前体素立方体的点云，然后将得到的重心合并成新的点云，该点云即为采样之后得到的稀疏点云数据。

传统的体素网格滤波法对原点云实现采样处理，是通过调整体素立方体大小的方式，得到合适的采样点集。该方法在减少三维点云数量的同时，保留了原始三维点云特征，保证新的三维点云与原始三维点云的点特征直方图在全局上近似。但这种方法需要手动规定体素网格立方体的边长大小，对于不同的点云数据，通过相同边长的体素网格立方体过滤之后的点云数量级将会是不确定的，而进行配准计算的点云数量级大小直接影响着算法的运算时间和运行效率，为有效控制三维点云配准算法的配准时间，需要得到确定的点云数量级大小。

因此本文在传统体素网格滤波法上进行改进，针对输入数量级未知的三维点云数据，自适应地求解出准确的体素立方体大小

maxts.t.F(k*ε+l)

(3)

式中：F表示体素网格滤波器，t表示动态迭代系数，ε表示体素立方体边长的变化步长，l表示初始立方体边长大小，Pmax表示通过滤波器采样后得到的点云数量最大值。

自适应体素网格滤波法算法流程[1]：

步骤1 设置ε用于规定变化步长；设置Pmax大小，用于获取期望的点云数量级。

步骤2 输入原始三维点云数据P。

步骤3 如果原始三维点云数据P的三维点数量小于Pmax，直接跳转到步骤5。

步骤4 如果点云数据P的点集数量大于Pmax，根据P的点集数量与Pmax的差值调整t，直到P的点集数量不大于Pmax为止。

步骤5 输出采样后的结果Pf。

使用本文提出的自适应体素网格滤波算法，在保证原始点云特征的前提下，能减少点云数量和计算时间，提高整个配准算法的效率。同时，本算法通过设定Pmax来确定滤波后得到的允许最大点云数量，解决了传统体素网格滤波器对于体素立方体边长的局限性，以自适应的体素立方体过滤出期望的点云数量。

2.2 特征点的提取

PFH作为一种较为健壮的三维点云特征描述符，用于表示查询点pq的局部几何形状特征。如图1所示，以查询点pq为圆心，r为半径形成的圆为pq点PFH特征的影响区域，在这个影响区域内，查询点与全部邻域点之间两两相连构成一个统一的集合。

图1 PFH邻域运算

通过图2的局部坐标关系，可以将相邻两个三维点Ps(xs,ys,zs) 和Pt(xt,yt,zt) 的六元组通过式(4)进行降维处理，转换为一个四元组 (α,φ,θ,d)

(4)

图2 局部坐标系

SPFH(simple point feature histograms)是在原有的PFH基础上进行简化而产生的，其根本目标是为了降低算法的运算时间复杂度。对于包含n个点的点云P的PFH理论时间复杂度为O(nk2)，其中k为近邻点个数。SPFH的算法思维是：查询点计算三维点特征直方图时将不会包含邻域点与领域点之间的关联信息，仅仅只是利用查询点与邻域点之间的关联信息，图3表示SPFH算法k邻域运算关系。

图3 SPFH邻域运算

本文改进的快速点特征直方图FPFH是PFH的简化模式，它的思想是分别计算查询点的k邻域内每个点的简化点特征直方图SPFH，并用距离的二次方作为衡量邻域点权重的系数。改进的FPFH为

(5)

式中：S(pq) 代表邻域内每个点的简单点特征直方图，k代表邻居点的数量，bi代表查询点和第i个邻居点之间的欧氏距离。相较于距离的一次函数，本算法使用的距离二次函数能进一步提升距离较近点的权值，同时弱化距离较远点的权值，对于查询点的特征表示更合理，使得对应点对的配对更加准确。

2.3 ICP算法

在经过粗配准之后得到的两片点云已经基本配准，但仍然存在偏差，配准精度也较低，为了进一步提高配准精度，还需要使用精配准算法，本文使用ICP配准算法。

算法概述：传统的迭代最近点ICP算法是一种十分经典的点云配准方法，将要拼接的两对点集按照最近点配对原则确立对应点对qi与pi，对应点对的个数为N。最后再使用最小二乘法来最小化原始三维点云和目标三维点云数据之间的距离，计算出最优的坐标变换矩阵，得到其旋转矩阵R和平移向量t，从而令误差函数式(6)能够取得最小值

(6)

式中： (qi,pi) 是目标点云和源点云的第i对点。

ICP算法步骤：

步骤1 初始输入数据量大小为n的三维点云数据P。

步骤2 经过最近邻搜索算法在三维目标点云Q之中找到与三维特征点pi对应的三维特征点qi, 使得pi与qi之间的欧几里得距离最小。如果某一点对之间的欧几里得距离满足给定的阈值，则将这对点作为对应点对进行保存。

步骤3 由三维特征点qi和三维特征点pi得到旋转矩阵R与平移向量t。

步骤4 将三维特征点云pi通过步骤3求得的R|t进行变换得到新的三维特征点云p′i。

步骤5 计算三维特征点p′i和三维特征点pi之间距离的均方误差值:dis。

步骤6 最后依据dis是否为小于原始给定的阈值，或者其配准的迭代次数是否已经达到最初给定的最大迭代次数。符合其中一项，则就会终止迭代，否则将继续反复迭代以上步骤。

2.4 本文算法描述

步骤1 输入原始三维点云数据集P，和目标三维点云数据集Q；

步骤2 预设一个通过滤波器采样之后得到的三维点云数量最大值Pmax，通过自适应体素网格滤波器滤波之后，得到一个轻量级且特征完整的三维点云数据Pf以及Qf；

步骤3 通过式(4)可以计算得到三维点云数据Pf和三维点云数据Qf内各邻域点对之间的α,φ,θ, 并存入点特征直方图所对应的子区间里；

步骤4 通过式(5)计算得到改进的快速点特征描述符P_fpfh(Pf的描述符)，Q_fpfh(Qf的描述符)；

步骤5 将含有特征描述符的点云进行采样一致性配准得到旋转平移矩阵sac_tran；

步骤6 导入待配准的三维点云数据，使用旋转矩阵sac_tran作为初始旋转矩阵，设置配准的最大迭代次数为10，两次迭代的欧几里得距离的最小变化量为1e-10， 进行迭代最近点配准；

步骤7 一旦达到最大迭代次数或最小变化值时，终止配准，输出最终配准完成的点云。

3 实验结果与分析

本文在CPU主频为2.6 GHz,win10系统笔记本进行实验开发平台的搭建，在Visual studio 2017软件上搭建了PCL点云库的环境进行实验。该实验采用的验证数据是来自斯坦福大学提供的公开三维点云数据模型。其中Bunny点云数据如图4所示，Bunny点云数据约有4万个点，序号分别为0和45，0号Bunny数据表示的是在规定零度位置采集到的点云数据，45号则表示在45度角位置采集到的点云数据模型。使用的衡量指标是点云数据之间的ESM均方误差以及该算法运算所需的配准时间，并与对比文献[13]进行比较。

图4 Bunny原始数据

首先将原始三维点云数据0号Bunny数据和45号Bunny数据通过自适应体素网格滤波器进行数据的预处理，得到的三维点云采样之后的结果如图5和图6所示，该方法采样之后，将会得到一个数据量级较小的采样数据组，再将该数据组进行改进的采样一致性点云配准，配准效果如图7所示。

图5 目标点云采样结果

图6 源点云采样结果

图7 本文改进算法配准结果

图5和图6中所示的结果是在设置Pmax=1000的条件下得到的采样结果，得到目标点云采样数据的点数为986个点，源点云采样数据的点数为948个。从采样结果不难看出，Bunny数据在经过自适应体素网格滤波器后，得到的点云数据量被大幅度的缩减，与此同时仍然保留了原始点云数据的基本特征。

通过图7可以看出采用本文提出的三维点云配准算法能够将两片Bunny三维点云数据进行准确配准，配准效果较好。由表1可知晓三维点云配准之后的点云之间的均方误差将会缩小到3.5625e-5。该实验的结果表明，本文所提出的改进的三维点云配准算法配准精确度高，表1中展示的是本文改进算法在精配准和粗配准之后的表现，与参考文献[13]进行比较，可以得出本文提出的三维点云配准改进算法无论是在进行配准消耗的时间上，还是配准的精度上都有较高的提升，在配准时间上减少了39.39%，三维点云配准的整体均方误差降低了54.65%。

为了验证本文提出的改进算法在不同三维点云数据集上的有效性，本文也将使用斯坦福大学所提供的3组公开数据集Happy(开心佛陀)、Dragon(龙)以及Armadillo(犰狳)进行三维点云配准实验，通过分析配准的均方误差以及配准所需要的时间来判定该算法的性能优劣。其中点云数据开心佛陀约有8万个点，使用的是第0号和第24号点云数据；点云数据龙约有4万个点，使用的是第0号和第24号点云数据；点云数据犰狳约有3万个点，使用的是第270号和第300号点云数据；实验结果如下：

通过结合图8和表2进行分析，评价指标为点云间距离的均方误差以及配准所需要的时间，可以看出本文提出的改进算法在针对不同数据集进行配准所需要的时间基本能控制在10 s左右，得到的误差均在1e-4 m内，配准精度高。

表1 本文改进算法配准结果与传统配准算法比较

图8 本文算法配准结果

为验证二次距离权值对本算法精度的提升效果，使用不同的Pmax使得Bunny点云数据量级变化，分别使用一次、二次、三次距离的FPFH特征直方图，进行特征表示，并完成配准，得到配准后的结果如图9所示。分析图9可以看出使用二次距离的FPFH特征直方图得到的配准精度更高。相较于原始的一次距离的FPFH，在其它条件相同的情况下，二次距离的FPFH在精度上最高能提升35.33%。

表2 本文算法在不同数据上的配准效果

图9 距离权值与Pmax关系

4 结束语

点云配准技术作为三维重建十分重要的一个环节，已经渐渐地成为三维重建行业研究的热点问题。本文针对传统算法收敛慢,算法复杂度高等问题，提出了自适应体素网格滤波算法，可以根据原始点云量级不同的大小自动修改体素立方体大小，有效剔除偏差较大噪点的同时还降低了点云数据量级，并能保留点云数据的根本特性，为后续配准阶段提供了一个可靠且轻量级的点云数据。除此之外，在快速点特征直方图求解阶段，引入距离的二次函数，优化邻域点的权值，提升距离较近点的权值，同时弱化距离较远点的权值，对于查询点的特征表示更合理，使得对应点对的配对更加准确，直接提高了配准的精度。实验结果表明，该改进算法相较于传统的点云配准算法在配准时间上减少了39.39%，整体配准均方误差提升了54.65%。

本算法与前人去年提出的点云算法相比，在配准时间和配准精度上都有所提升，但依然有待解决的问题。对于本文所提出的自适应体素网格滤波法，虽然能降低点云数据的数量级，但在使用网格点集的重心作为网格内其余所有点的替代点，难免会引进新的误差，这是在配准精度与配准效率上进行权衡的结果。本文算法在采样一致性点云配准算法上做了改进，配准精度以及配准时间都有所提升，但总体运行时间仍然为秒级，无法实现实时点云配准，将来会持续致力于提升配准算法的效率。