基于多核并行和动态阈值的点云配准算法

李运川,王晓红,陈思吉,葛义攀,李 闯

(1.贵州大学矿业学院,贵州 贵阳 550025; 2.贵州大学林学院,贵州 贵阳 550025)

0 引 言

点云配准是指将不同视角获取的同一物体点云数据变换到统一坐标系的过程[1]，该技术在三维重建[2]、逆向工程[3]、文物保护[4]、地质灾害[5]、无人驾驶[6]等领域具有广泛应用。为了解决点云配准问题，国内外研究者提出了多种算法[7-8]。其中，具有代表性的算法有采样一致性初始配准算法(Sample Consensus Initial Aligment, SAC-IA)[9]、四点快速匹配算法(4-Points Fast Matching Aligment, 4PCS)[10]、迭代最近点算法(Iterative Closest Point, ICP)[11-14]和正态分布变换算法(Normal Distributions Transform, NDT)[15-17]等。

同时，针对点云配准算法存在错误匹配点对、精度不高等不足，国内外研究者实现了多种改进算法。陈学伟等人[18]利用采样一致性初始配准算法(SAC-IA)实现两点云的粗配准，然后利用KD树加速查找对应点对，并使用方向向量阈值去除错误对应点对，以此改进ICP算法，从而实现精配准。实验表明该算法具有相对较好的配准精度和收敛速度。王宗跃等人[19]提出了基于多核CPU的海量点云并行K近邻算法，该算法利用多核CPU的多线程并行特性，提高了K邻近搜索速度。张蕾等人[20]提出了一种基于距离约束改进的ICP算法，该算法利用邻近点法来获取配准点，并使用这些配准点的重心作为参照点，求取各个配准点到配准点重心之间的距离，再结合点对距离约束来剔除错误配准点对，最后，进行点云配准。该算法在配准速度和精度方面均有所提高。钟莹等人[21]在匹配点对集中，通过增加阈值来降低噪声对配准的影响，并对点与点之间的方向向量夹角设定阈值，从而达到提高配准精度、剔除错误匹配点对的目的。

针对点云配准中存在错误匹配点对、精度不高等问题，本文采用基于OpenMP的多核并行技术[22-23]来提升配准算法速度，并引入动态阈值和配准点重心约束来剔除错误匹配点对，以此来提升配准精度，从而完成点云配准。通过对ICP、SAC-IA+ICP算法和本文算法进行点云配准实验，并从算法速度、精度等方面进行对比分析，来验证本文算法的可行性。

1 本文点云配准算法的流程

本文的点云配准算法包括2个过程：粗配准和精配准。在粗配准阶段，首先进行点云下采样，然后利用基于OpenMP的多核并行技术，来计算点云查询点的法向量、FPFH特征，以及并行查找对应点对，最后计算出粗配准变换参数，从而实现粗配准。精配准采用改进的ICP算法逐步获取最佳配准结果。本文点云配准算法的流程如图1所示。

图1 本文点云配准算法的流程

2 粗配准算法

本文采用改进的SAC-IA算法进行点云粗配准，改进点主要体现在利用OpenMP并行加速提取点云查询点的法向量、FPFH等特征，以及对对应点对的并行查找。

2.1 基于OpenMP加速特征提取

在利用SAC-IA算法进行粗配准时，提取点云查询点的法向量、FPFH等特征以及查找对应点对的时候，各查询点的相关计算存在时间复杂度高的问题，尤其在查找对应点对时花费了粗配准的大部分时间，而利用OpenMP的多核并行优势，正好可以解决时间复杂度高的问题。此外，各点云查询点的特征提取是相互独立的，可以同时进行，这与OpenMP的多核并行特性不谋而合。

通过上述分析，本文利用OpenMP来实现SAC-IA算法中所涉及的点云查询点的法向量、FPFH等特征提取，以及对应点对的查找这3个部分的并行加速处理，从而提升粗配准算法的速度。其具体过程如下所示，并且可以用图2清晰地表示其流程。

1)第1个并行域实现点云查询点的法向量的并行加速提取，由于每个点云查询点的法向量计算是相互独立的，可以利用多核多线程并行加速提取法向量特征。

2)在第2个并行域中，基于步骤1求得的法向量特征信息，对点云查询点的FPFH特征进行并行加速提取，由于每个点云查询点的FPFH计算相互独立，因此每个线程负责并行加速提取FPFH特征。

3)基于前2个步骤提取的法向量、FPFH特征信息，在第3个并行域中，实现对应点对的并行加速查找，由于对应点对查找中存在多个可以并行的循环操作，这正好可以利用OpenMP来并行加速这些循环操作。

图2 基于OpenMP加速特征提取

2.2 改进的SAC-IA算法

本文采用OpenMP来改进SAC-IA算法，以此改进算法加速计算源点云P和目标点云Q的粗配准变换参数。本文改进的SAC-IA算法的主要步骤包括：点云下采样、基于OpenMP实现点云查询点的法向量和FPFH特征的并行加速提取、基于OpenMP并行加速查找对应点对、计算粗配准变换参数R、T，其大致思路如下：

Step1对点云进行下采样，并基于OpenMP并行加速提取点云查询点的法向量、FPFH等特征；从源点云P中选取N个样本点，为了尽量保证所选的样本点具有不同的FPFH特征，样本点两两之间的距离应大于预先给定的最小距离阈值dmin。

Step2基于OpenMP并行加速查找对应点对，在目标点云Q中查找与源点云P中所选样本点具有相同或相似FPFH特征的一个或多个点，从这些点中随机选取一个点作为源点云P在目标点云中的匹配点，从而获得N对匹配点对。

Step3计算Step2所获得的N对匹配点对之间的刚体变换参数,然后通过求解匹配点变换后的距离误差和函数L(li)来判断当前刚体变换参数的性能；此处的距离误差和函数L(li)多使用Huber罚函数表示，其公式如式(1)和式(2)所示：

(1)

(2)

其中，ml为预先给定值，li为第i组匹配点变换之后的距离差。

Step4重复Step1～Step3，并对比所有的刚体变换参数，使得距离误差和函数L(li)的值最小，从中找到一组最优粗配准变换参数R、T。

Step5利用Step4获得的最优粗配准变换参数R、T，将源点云P配准到目标点云Q，获得新的点云P。

3 精配准算法

在第2章中实现了点云粗配准，这就为当前的精配准提供了较好的初始姿态。接下来，将采用一种改进的ICP算法来实现点云精配准，即在ICP算法的基础上引入配准点重心约束和动态阈值来剔除错误对应点对，从而获得更高的配准精度和更好的配准效果。

3.1 配准点重心约束

由于在点云数据的采集过程中不可避免地掺有噪声，且点云数据重叠区域的拓扑结构并不是完全一致的，因此，在确定初始匹配点对的时候，往往存在一对多或错误对应的情况，这会使得点云配准的精度降低或者无法配准。为此，本文以“配准点重心”(按式(3)计算)为参照点，计算初始匹配点对与配准点重心的距离差值，若距离差值大于设定的动态阈值，则视为错误匹配点对，予以剔除；反之，则保留为正确匹配点对，以此实现错误匹配点对的剔除，公式如式(4)所示：

(3)

(4)

3.2 动态阈值

通过文献分析，本文发现剔除错误匹配点对时所用的阈值是固定阈值，而固定阈值法的缺陷在于固定阈值不够灵活，它只适用于某个点云模型配准，适应性较差，对于多种点云模型时，这会导致误剔除率增大。对此，本文以动态阈值作为约束条件，当参与配准的点云模型发生变化或ICP算法不断迭代时，阈值也会自动变化进行适配，从而更好地剔除错误匹配点对。动态阈值法的具体原理如下：

Step1按照公式(5)计算初始匹配点对集中的各对匹配点到各自的配准点重心的距离。

(5)

其中，ai和bi为源点云和目标点云中第i个对应点到配准点重心的距离，N为初始匹配点对集中匹配点对的数目。

Step2按照公式(6)计算各对匹配点到各自的配准点重心的距离的差值，以及该距离差值的平均值。

(6)

Step3利用公式(7)和公式(8)计算各对匹配点到各自的配准点重心的距离的差值的中误差。

(7)

(8)

其中，m为各对匹配点到各自的配准点重心的距离的差值的中误差，N为初始匹配点对数目。

Step4设定阈值。通过实验可以发现：先采用测量中通用的2m为阈值，将2m作为阈值时，仅剔除了较少的错误匹配点对，收敛速度也无明显增加；而阈值设置为1.5m时,剔除了较多错误匹配点对，收敛速度得到一定的提高；故设定1.5m为阈值。

3.3 改进的ICP算法

源点云为P，目标点云为Q。改进的ICP算法步骤如下：

Step1对原始点云进行下采样。利用VoxelGrid滤波器对源点云P和目标点云Q进行下采样，获得精简后的源点云P和目标点云Q。

Step2确定初始对应点对集。对精简后的源点云P中的每一个点，按照欧氏距离最近的原则，在目标点云Q中用KD树加速查找对应点，从而得到初始对应点对集。

Step3计算配准点重心。对初始对应点对集，分别计算源点云P和目标点云Q的配准点重心。

Step4剔除错误对应点对。利用“配准点重心约束”剔除错误对应点对，得到正确对应点对集。

Step5求解刚体变换矩阵。在Step4所获得的正确对应点对集上，使用奇异值分解法求得使目标函数最小的刚体变换矩阵(R,T)，计算公式如式(9)：

(9)

其中，(pi,qi)是正确匹配点对集中的第i对匹配点对，N为正确匹配点对的数目。

Step6更新源点云。使用Step5得到的刚体变换矩阵R、T，将源点云P配准到目标点云Q，获得新的源点云P；然后，重复Step2～Step6进行迭代计算，直到达到最大迭代次数、均方误差MSE小于误差阈值δ或者相邻两次变换矩阵差值小于所设阈值ε(见式(10))，则停止迭代，然后输出最终刚体变换参数。

(10)

其中，pi为第k次迭代后的更新源点云Pk中的第i个点，qi为目标点云Qk中的第i个点；N是正确匹配点对数目；MSEk是第k次迭代后目标点云与更新源点云之间的平均距离平方和(即均方误差)；MSEk-MSEk+1为2次迭代误差。

Step7按照最终的刚体变换参数R、T，将源点云P配准到目标点云Q。

4 实验与分析

4.1 实验环境与数据

本实验利用斯坦福大学3D模型库数据进行实验，实验所采用的软硬件环境为：3.40 GHz的AMD Ryzen 5 2600的CPU，8 GB内存；64位的Windows10操作系统，以及Visual Studio 2017，并结合点云库PCL1.9.1实现本文点云配准算法。

4.2 点云配准实验

为了进行对比实验，本文使用了马、兔、头像这3组点云数据。选择这3组数据来完成本文实验的原因有3点：1)该数据是点云配准算法研究中的经典数据，且适用于本文研究；2)该数据的数据量都在几万到十万个点之间，可以用来验证本文算法的速度性能；3)该数据没有噪声，可以用于人为添加噪声来验证算法的抗噪性。在具体实验中，首先，采用本文点云粗配准算法对点云数据进行粗配准处理；然后，采用本文点云精配准算法进行点云精配准。同时，与ICP、SAC-IA+ICP算法进行对比实验。图3(a)是待配准马点云模型，图3(b)、图3(c)分别是ICP、SAC-IA+ICP算法配准结果，图3(d)是本文算法配准结果。图4(a)是待配准兔点云模型，图4(b)、图4(c)分别是ICP、SAC-IA+ICP算法配准结果，图4(d)是本文算法配准结果。图5(a)是待配准头像点云模型，图5(b)、图5(c)分别是ICP、SAC-IA+ICP算法配准结果，图5(d)是本文算法配准结果。

(a) 待配准点云 (b) ICP (c) SAC-IA+ICP (d) 本文算法

(a) 待配准点云 (b) ICP (c) SAC-IA+ICP (d) 本文算法

(a) 待配准点云 (b) ICP (c) SAC-IA+ICP (d) 本文算法

4.3 实验分析

4.3.1 配准效果分析

由图3(a)～图5(a)可以直观地发现待配准的马、兔、头像点云数据的初始姿态存在明显偏差；从图3(b)～图3(d)、图4(b)～图4(d)和图5(b)～图5(d)的配准结果可以看出，对于初始姿态并不好的待配准点云，相较于ICP、SAC-IA+ICP算法，本文算法实现了更好的配准效果。

4.3.2 配准精度分析

从表1可以看出，相较于ICP、SAC-IA+ICP算法，本文算法在进行马、兔、头像点云模型配准时，在配准速度提升的情况下，配准精度也获得了提升。与ICP算法相比，在马、兔、头像点云配准中，本文算法配准精度都至少提升了35%；与SAC-IA+ICP算法相比，在马、兔、头像点云配准中，本文算法配准精度分别提升了20%、3.5%和4.9%。本文算法之所以获得配准精度的提升，是由于在ICP算法中引入基于动态阈值的配准点重心约束，从而剔除了错误匹配点对，使得因错误匹配点对的存在而引起配准效果不佳的问题得到改善。

表1 配准精度

4.3.3 配准时间分析

从表2可以看出，与ICP、SAC-IA+ICP算法相比，本文算法在提升配准精度的情况下，速度也得到了提升。相较于ICP算法，本文算法在进行马、兔、头像点云配准时，配准速度分别提升了12%、46%、17%；相较于SAC-IA+ICP算法，本文算法在进行马、兔、头像点云配准时，配准速度分别提升了6.9%、29%、12%。在配准过程中，为了剔除ICP算法中的错误匹配点对，本文引入动态阈值的配准点重心约束，这确实增加了单次迭代的耗时，但总的配准耗时降低了。究其原因，一方面是由于迭代次数减少了，另一方面要归功于利用OpenMP实现点云查询点的法向量、FPFH等特征的并行加速提取以及对应点对的并行查找，从而降低了算法耗时。此外，在整个算法中，对点云进行了精简，这减少了配准的计算量，同时加入了KD树，这加快了对应点的搜索速度，从而提升了本文配准算法的速度。

表2 配准时间

4.3.4 抗噪性分析

如图6(a)所示，本文以兔点云为实验对象，然后给其分别添加10%、20%、30%和40%的高斯噪声，来验证本文算法的抗噪性。图6(b)和图6(c)分别是SAC-IA+ICP和本文算法在带有不同比例高斯噪声下的配准结果。通过表3可以看出，随着高斯噪声比例的增加，SAC-IA+ICP和本文算法的配准精度都不断上升，而且本文算法与SAC-IA+ICP算法的配准精度能够保持一致，这说明本文算法具有与SAC-IA+ICP算法相当的抗噪性。

(a) 输入数据

(b) SAC-IA+ICP算法

(c) 本文算法图6 高斯噪声下的点云配准

表3 不同高斯噪声比例下的配准精度

5 结束语

点云配准是三维重建、逆向工程、文物保护、计算机视觉、自动驾驶等领域的关键技术，点云配准算法的配准精度、速度会直接影响后续处理的质量。针对点云配准中存在错误对应点对、精度不高等问题，在粗配准阶段，本文采用基于OpenMP的多核并行技术，来并行加速提取点云查询点的法向量、FPFH等特征，以及对对应点对的并行查找，从而提升了配准过程的速度；在精配准阶段，引入基于动态阈值的配准点重心约束，结合点对距离约束来剔除错误匹配点对，从而达到提升整个配准算法精度的目的。实验结果表明，相较于ICP、SAC-IA+ICP算法，本文算法在提升配准精度的情况下，配准速度也得到了提升；在抗噪性方面，本文算法具有与SAC-IA+ICP算法相当的抗噪性。此外，本文算法的速度还具有提升空间，因此在下一步工作中，将考虑利用CUDA并行加速点云配准算法，以此实现算法速度的更大提升。