基于FPFH特征与ICP算法结合的三维点云配准算法

基于 FPFH特征与 ICP算法结合的三维点云配准算法

王玮婕，薛河儒 *（通讯作者），杨彤

内蒙古农业大学 计算机与信息工程学院，内蒙古自治区 呼和浩特 010018

摘要 对初始位置完全未知且距离较远的点云配准时，传统ICP算法容易出现匹配偏差、错误等结果。针对这一问题，提出了基于快速点特征直方图的采样一致性配准的粗配准方法。对待配准点云构建快速点特征直方图，基于此特征利用采样一致性算法实现粗配准，然后再使用ICP算法实现精配准。实验结果证明，该方法与传统ICP算法相比，能改善点云匹配准确度，得到正确的配准结果。

关键词：FPFH 采样一致性算法 ICP算法 三维点云配准

Three-dimensional point cloud registration algorithm based on the combination of FPFH features and ICP algorithm

Wang Weijie Xue Heru* Yang Tong

（College of Computer and Information Engineering, Inner Mongolia Agricultural University, Hohhot 010018,Inner Mongolia,China）

Abstract When registering a point cloud with a completely unknown initial position and a long distance, the traditional ICP algorithm is prone to matching deviations and errors.To solve this problem, a rough registration method based on sampling consistent registration of fast point feature histogram is proposed.A fast point feature histogram is constructed for the point cloud to be registered, based on this feature, a sampling consensus algorithm is used to achieve coarse registration, and then an ICP algorithm is used to achieve fine registration.Experimental results prove that compared with the traditional ICP algorithm, this method can improve the accuracy of point cloud matching and obtain correct registration results.

Keywords FPFH Sampling consensus algorithm ICP algorithm 3D point cloud registration

基金项目：国家自然科学基金项目（61461041）

1. 引言

目前，三维激光扫描技术迅猛发展，随之利用各种三维激光扫描仪获取点云数据，对物体进行三维重建成为研究热门^[1]。其中手持式三维激光扫描仪应用最为广泛，因为其采用非接触式的扫描方式，便捷，速度快，分辨率高^[2]。但在扫描过程中，由于物体遮挡及设备自身原因，一次扫描无法得到完整的点云，需要多次多角度的对同一物体进行扫描^[3]。为了获取完整的物体模型点云数据，将多次扫描的数据通过转换整合在同一坐标下，这一过程就是点云数据预处理中最重要环节—点云配准^[4]。

国内外学者研究最广的点云配准算法为迭代最近点算法（ICP）及其各种改进算法。ICP算法最早由Besl P J和Mckay H D^[5]提出，此算法对待配准点云的初始位置要求极高，点云距离较近且位置相似时，ICP算法配准具有较高的准确率；点云距离较远且相对位置完全未知时，该算法则会出现局部最优问题^[6]。为了解决这一问题，本文提出一种粗配准与精配准结合的算法。首先提取待配准点云的快速点特征直方图，使用采样一致性算法进行粗配准，然后使用ICP算法进行精配准。该结合算法在缩短运行时间的情况下提高配准精度。

2. 点云配准算法

本文提出基于快速点特征直方图（FPFH）特征的采样一致性算法（SAC-IA）的点云粗配准，与ICP算法精配准相结合的点云配准方法。首先估计待配准点云数据的法向量，再使用k-d树加速计算待配准点云的FPFH特征。然后根据FPFH特征，使用采样一致性算法完成粗配准。最后，基于此再使用ICP算法，通过不断迭代使得点云空间位置差别最小化，完成点云精配准，具体流程图如图。

图1 FPFH-ICP配准流程图

1. 基于FPFH特征的SAC-IA点云粗配准

SAC-IA点云粗配准算法^[7]是通过相似的点云特征匹配来进行配准，借助点云FPFH特征直方图的相似程度来搜索两个待配准点云数据间的对应点，通过计算特征，完成粗配准。已知两个待配准点云分别为 和 ，首先计算两个点云的FPFH特征，然后采用SAC-IA算法完成粗配准，详细步骤如下：

1）从点云 中选取 个采样点，且 个点中任意两个之间欧式距离必须大于提前设定的最小距离阈值 ，以此保证采样点FPFH特征的差异性。

2）搜索点云 中与采样点FPFH特征相似的点，将它们看成点云 在点云 中的对应点。

3）通过对应点计算点云 和点云 之间的刚体变换矩阵，并采用Huber罚函数来评判配准时带来的误差，Huber罚函数的公式如下所示： （1）

（1）式中， 为提前设定值， 为第 组对应点经过变换的距离差。

4）重复1）~3），直到获得误差最小时对应的变换矩阵，即所需的最优变换矩阵。

1. 点特征直方图

点特征直方图^[8]（PFH）是参数化查询点与其 领域内点法线角度差异，最终形成一个多维直方图来表示点与其 领域点的几何属性。此直方图对点云曲面6维姿态具有不变性且在不同采样密度或噪声下具有鲁棒性。

PFH计算方法如下：

1）假设查询点为 ，以 为圆心，以 为半径构造一个圆形区域，查询 的 近邻点，如图2为PFH计算的影响范围。

图2 PFH计算的影响范围图

2）计算球内 近邻点的法向量，假设其中两点 和 （i不等于j）的法向量分别为 和 ，以 的法向量 为 轴，根据右手定则建立一个局部坐标系。坐标轴计算规则如下：

（2）

公式（2）中， 是指 和 两点之间的欧式距离。

3）根据上述坐标系建立 和 两点的局部坐标系如下图3。

图3 局部坐标系建立图

4）法向量 和 之间的差异使用三元素 表示，计算公式如下所示：

（3）

（4）

（5）

5）对于图2中查询点 的 邻域内所有点对，计算上述4）中的三元素。在三维空间中，每个点的位置和法线空间信息都需要3个参数 ，因此三元素中三个参数可以包含两个点的所有信息。

6）将上述三个参数的取值范围分别等分为 个子区间，再分别统计每个参数落在子区间的点数量，最后根据各子区间点数量的占比来构建查询点 的PFH特征直方图。

2. 快速点特征直方图

快速点特征直方图^[9]（FPFH）算法是根据PFH简化而来的，既能保留PFH特性，又能提高运算复杂度。它的主要思想是只计算查询点和其 邻域内点之间的三元素，称为SPFH，再通过一个公式将所有的SPFH加权计算得到FPFH特征。图4为FPFH计算影响范围图。

图4 FPFH计算影响范围图

FPFH计算方法如下：

1）对于查询点 ，计算其 ；

2）重新定义每个点的 邻域，通过公式（5）利用 加权计算FPFH特征。

（6）

（6）上式中， 是 和 之间的欧式距离。

2. 基于ICP算法的点云精配准

粗配准已经使得两个点云数据大致重合，但是角度和位置仍具有偏差，还需使用精配准来提高精度缩小误差。在精配准中，使用最多的就是迭代最近（ICP）算法^[10]。ICP算法主要是通过最小二乘法不断对点与点之间进行旋转和平移，使得两个点云的匹配误差最小。ICP算法的实现流程具体如下：

1）将经过粗配准的点云 （变换后的点云 ）和点云 作为精配准的待配准点云；

2）从点云 中选取点子集 ，在点云 中搜索与 中每个点都距离最近的点，记作 ， 和 对应关系满足 ；

3）根据2）中对应关系计算旋转矩阵 和平移矩阵 ，使得误差函数取得最小值，误差函数表示为：

（7）

4）利用旋转矩阵 和平移矩阵 对点子集 进行更新，得到点子集 ，满足：

（8）

5）使用点子集 继续重复2）~4），计算平均距离 ，表达式为：

（9）

6. 通过比较 与提前设定的阈值 来判断是否结束迭代：若 ，则继续迭代；若 ，或者首先达到预先设置的迭代次数k，则满足收敛，结束迭代。

3. 结果与分析

研究采用手持式三维激光扫描仪扫描鼢鼠头骨上颌标本，获取的腹面观和背面观点云数据经过预处理后作为待配准点云。如图5所示，位于左边的腹面观点云，位于右边的是背面观点云。使用硬件配置为Intel(R) Core(TM) i7-6700 CPU @ 3.41GHz,8.00GB内存的Windows10 64位系统的计算机进行试验，应用Visual Studio 2017、开源点云库pcl1.8.1进行编程实现点云配准。

图5 待配准点云

1. 传统ICP算法配准

对待配准点云数据直接采用ICP算法，结果如图6所示。实验中设置最大迭代次数为3800次，两次变化矩阵之间差值为1e-20，均方误差为1e-5。从图6中可以看到，两个点云匹配结果严重错位，配准误差为3.88，出现了局部最优的问题，配准效果很不理想。

图6 传统ICP算法配准结果

2. FPFH-ICP算法配准

对待配准点云首先借助FPFH特征利用采样一致性算法进行粗配准，配准结果如图7所示。实验中设置搜索半径为20，每次迭代计算中使用的样本数量为20。从图7可以看出，待配准点云已经大致重合，配准误差为1.29，只有牙齿和颧骨处仍有偏差。基于粗配准的结果，再采用ICP算法进行精配准，配准结果如图8所示。从图8可以看出，牙齿和颧骨部分已没有偏差，鼢鼠头骨上颌腹面观和背面观已经完全匹配，可以得到完整的头骨上颌点云。经过粗配准后再使用ICP算法的配准误差为0.34。

图7 粗配准结果 图8 ICP精配准结果

表1 本文算法与传统ICP算法的对比

从表1可以看出，无论是在配准误差还是运行时间上，本文提出方法的结果比直接使用传统ICP算法进行配准的结果都更胜一筹。从配准误差可以明显的看出传统ICP算法在解决初始位置完全未知且距离相差较远的点云配准问题上效果不好，误差较大，无法得到精确地配准结果。而本文提出的先使用粗配准算法使待配准点云大致重合，具有相对较好的初始位置，再使用ICP算法对其进行精配准的算法，可以缩小误差，提高配准的精度。

4 结语

本文针对传统ICP算法在解决待配准点云初始位置完全未知且距离相差较远问题时容易出现匹配错误等问题，提出了基于FPFH特征的采样一致性算法的粗配准与传统ICP算法精配准相结合的方法，并基于PCL库通过VS2017实验加以验证。根据实验配准结果来看，本文算法在配准误差和运行时间上都优于传统ICP算法，能有效解决匹配错误等问题。但是，此方法仍具有缺陷，没有考虑到错误点对对配准精度的影响。在日后的研究中，需要继续完善该方法，剔除错误点对的影响，进一步提高匹配精度。

参考文献

1. Jin W , Xing H , Wei Q . A Survey Study of the Blast Furnace at Kuangshan Village Using 3D Laser Scanning[J]. JOM, 2017, 69(1):64-70.

2. 倪春杰,胡彦萍.手持式三维激光扫描仪数据拼接技术与实践[J].机械研究与应用,2018,31(05):173-175+178.

3. 范哲君. 散乱点云自动配准算法研究[D].哈尔滨工程大学,2019.

4. 徐兆阳. 三维重建中的点云配准技术研究[D].电子科技大学,2020.

5. Besl P J , Mckay H D . A method for registration of 3-D shapes. IEEE Trans Pattern Anal Mach Intell[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2):239-256.

6. 程亚丽. 三维激光扫描点云数据配准算法研究[D].昆明理工大学,2017.

7. 侯彬,金尚忠,王赟,陈智慧,曹馨艺.点云配准方法在粗配准中的比较[J].激光与光电子学进展,2020,57(08):294-301.

8. Rusu R B , Blodow N , Marton Z C , et al. Aligning Point Cloud Views using Persistent Feature Histograms[C]// 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, September 22-26, 2008, Acropolis Convention Center, Nice, France. IEEE, 2008.

9. Rusu R B , Blodow N , Beetz M . Fast Point Feature Histograms (FPFH) for 3D registration[C]// IEEE International Conference on Robotics & Automation. IEEE, 2009.

10. Besl P J . A method for registration of 3d shapes[J]. IEEE Trans on PAMI, 1992, 14.