基于PCL的点云数据粗配准算法研究

PCL-Based Coarse Registration Algorithm for Point Cloud Data

摘要: 传统ICP算法精度受点云初始位姿影响较大，收敛速度慢，不能满足精细化点云建模的要求。基于此问题，通过基于快速点特征直方图的采样一致性配准方法进行粗配准。首先将两帧待配准点云进行体素滤波，其次进行表面法向量估计并计算关键点的PFPH特征，然后运用采样一致性算法得到最优变换，最后在此基础上再进行ICP配准。实验表明，该方法能有效改善配准精度和收敛速率。

Abstract: The accuracy of the traditional ICP algorithm is greatly affected by the initial pose of the point cloud, and the convergence speed is slow, which cannot meet the requirements of refined point cloud modeling. Based on this problem, the coarse registration is performed by the sampling consistency registration method based on the fast point feature histogram. Firstly, two pieces of the experimental point cloud to be registered are subjected to voxel filtering, followed by surface normal vector estimation and calculation of the fast point feature histogram of the key points, and then the sampling consistency algorithm is used to obtain the optimal transformation. Finally, the ICP matching is performed on the basis of this quasi. Experiments show that this method can effec-tively improve the registration accuracy and convergence rate.

1. 引言

三维点云建模所采集的数据不仅可以记录物体的空间信息，而且能够获取物体表面的几何信息，因此它已经成为近年来的研究热点 [1] 。在实际扫描中，由于物体的复杂性、仪器设备等因素的影响，单视角扫描只能获得某个视角的数据，而此视角的数据坐标是基于当前的仪器坐标系的，因此需要从不同方位对物体进行扫描，然后将不同方位下采集的数据统一到一个坐标系下 [2] 。

PCL是跨平台的开源点云库。PCL最开始主要应用于机器人研究应用领域，目前在CAD、逆向工程、激光遥感测量、虚拟现实等领域的应用也逐渐展开。为了便于开发，该库被划分为模块化的代码库，每个代码库都可以进行单独的编译 [3] 。根据不同基类对应的不同算法，PCL将点云数据处理的流程进行整合，可使代码更加简洁，算法结构清晰。PCL点云库的模块化使得算法实现过程更加紧凑和清晰，有助于解决三维激光点云的数据处理问题。

点云数据粗配准的基本思路是在缺少初始位姿信息的情况下，将两片点云大致粗略的配准到一个坐标系。在此基础上，ICP及其改进算法配准的精度才能得到保证。近年来，国内外学者对粗配准算法进行了比较深入的研究。张学昌等提出的配准算法是以扩展高斯球为基础的 [4] 。左超等提出的方法以点云的空间分布稀疏状态来控制某一片点云的旋转角度，在熵函数的基础上分析点云空间分布状态，提出了一种ILSDE粗配准方法 [5] 。该算法在误差允许范围内能有效实现粗配准。顾旭波等提出在点云数据中利用SIFT算法对关键点进行检测和筛选，随后运用曲面二次拟合计算其中点的主曲率，最后在设置阈值的基础上将主曲率与极值相对比，从而完成初始配准 [6] 。本文采用基于PFPH (快速点特征直方图)与采样一致性相结合的粗配准方法，可以有效提高点云配准的精度和收敛速度。

2. 点云粗配准

2.1. 体素下采样

在三维激光扫描设备进行采集数据工作时，由于设备所带传感器的特性以及扫描设备与被扫描物体的相对移动的影响，点云中通常存在一些噪声点和离群点。为了提升后续点云处理及建模的效率，需要采取一些措施对点云进行滤波。本文采用PCL中的体素网格法VoxelGrid类用于实现滤波，该方法可以在稀疏点云的同时不破坏点云的微观结构。体素网格法的思路是用一个立方体来封装点云数据，再根据点云的实际情况设置合适的参数对其进行分割，本文设置的参数为0.05，即子立方体的边长均为0.05 cm。在各个子立方体中计算所有点的重心，用此重心来近似表示子立方体中的其他点。采样步骤如下：

1) 确定立方体的边长。子立方体的边长

$L=a\sqrt[3]{s/c}$ (1)

参数a是调节子立方体边长的比例因子，s为比例系数，c为子立方体包含的点云数量

2) 正六面体的体积为 $V=L_X{L}_Y{L}_Z$ ， $L_X,L_Y,L_Z$ 分别为点云在X、Y、Z轴的最大值。

子立方体的点云数量为： $C=N/V$ ，其中N为点云总数

由此可得

$L=a\sqrt[3]{s{L}_x{L}_Y{L}_Z/N}$ (2)

然后把点云数据划分为e * f * g个子立方体，e、f、g的值通过取整函数来计算。

3) 根据前面的计算可以求得子立方体的重心，从而实现点云的下采样。

本文采用的两个原始点云数据分别有40,256和40,097个点。首先在VS2010中编译PCL中的VoxelGrid类代码，根据需要修改不同的参数，然后通过生成的可执行文件在CMD中输出体素滤波的结果如图1。经过体素滤波后实验点云数据分别为394和376个点，这对后面提高点云数据的处理速度是很有帮助的。

Figure 1. Voxel filter output

图1. 体素滤波输出结果

2.2. 点云法向量估计

外业采集的点云数据无法获取点云之间的邻近关系，因此点云处理之前需要建立点云的拓扑结构，其中法向量的估计是不可忽视的。虽然从仪器扫描的深度图像中也可获得法向量，但由于点云噪声、扫描角度的限制，以及图像的不连续等影响，通过这种方式获得的法向量无法满足三维重建的精度要求。为了求点云中一点的法向量，可用该点及其邻域拟合一个曲面，然后用曲面的切平面法线来代替，这就相当于一个最小二乘平面拟合的问题 [7] 。计算法向量的步骤如下：

1) 首先确定点P的K近邻元素 $\left\{p_1,p_2,p_3,\cdots \right\}$ ，计算出这些点的重心；

$0=\frac{1}{k}{\displaystyle {\sum }_{i=1}^k{p}_i}$ (3)

2) 计算最小二乘拟合法向量可转换为以下目标函数的最小值

$f={\displaystyle {\sum }_{Nbhd}\Vert \left(p_i-o\right)\cdot n\Vert }$ (4)

3) 最后，求解上式的最小值即计算协方差矩阵的最小特征值，此特征值所对应的特征向量即为所求。

在PCL中可用以下函数来估计协方差矩阵：

Eigen::Mattrix3fcovariance_matrix;

Eigen::Vector4fxyz-centroid;

Compute3DCentroid(cloud，xyz_centroid);

ComputeCovarianceMatrix(cloud,xyz_centriod,convariance_matrix);

2.3. 估计点云PFPH特征

在表示一个点的K邻域的几何特征中，由于许多场景中的特征点数量繁多，这些特征点所拥有的特征值大多数都相同或者相当接近，对这些场景利用法线和曲率来表示点特征，无法满足获取全局的特征信息的要求。快速点特征直方图(FPFH)与点特征直方图(PFH)相比计算复杂度明显降低，但保留了其大部分识别特性 [8] 。FPFH的计算步骤如下：

1) 计算每个查询点P_q与其邻近点的关系(这里用元组代表查询点与其某个邻域点所对应法线的偏差，用一组角度α、Φ、θ表示)，结果称为SPFH (简化的点特征直方图)；

2) 重新确定每个点的K邻域，P_q的最终直方图由邻近点的SPFH值来求得，公式如下：

$\text{FPFH}\left(P_q\right)=\text{SPFH}\left(P_q\right)+\frac{1}{k}{\displaystyle {\sum }_{i=1}^k\frac{1}{{\omega }_k}\cdot \text{SPFH}\left(P_k\right)}$ (5)

2.4. 基于采样一致性的粗配准

对于点云数据的粗配准来说，粗配准时利用所有的点来获得可能的对应点对，计算工作会比较复杂，同时会有一定几率造成局部最优 [9] 。针对此缺点，运用采样一致性方法会有一定的改进。不同于必须知道有限个对应关系的组合，该方法只需要维持相同的对应关系的几何关系。算法过程如下：

1) 从源点集P中选取m个样本点，并且保证它们的配对距离小于预先设定的最小阈值d_min在目标点集Q中找到与P中样本点的快速点特征直方图相似的点，在找到的这些点中随机选取一些点，这些随机选取点与样本点形成一一对应的关系。

2) 在目标点集Q中找到与P中样本点的快速点特征直方图相似的点，在找到的这些点中随机选取一些点，这些随机选取点与样本点形成一一对应的关系。

3) 计算对应点的刚体变换矩阵，这时通过将变换矩阵的误差与预先设定的用来衡量误差的阈值进行比较，以此来确定该矩阵是否为最佳变换。

3. ICP配准

粗配准让待配准的两片点云位姿尽可能的靠近，可减少待配准点云平移和旋转误差。但还需要ICP算法将两片点云的重合部分进行整合，从而实现精细配准。ICP算法首先根据最邻近的搜索从两待配准点云中建立相对应的点集P与Q，其中对应点对的个数为n。然后计算所有对应点的欧式距离的平方的平均值，通过不断迭代使这个平均值最小，平均值最小后就认为完成了配准 [10] 。ICP算法的基本思想是：

1) 在待配准点云P选取点集p_i点，然后在点云Q中找到相对应的邻近点云，要求 $\Vert p_i-q_i\Vert =\min$ ；

2) 给定误差函数

$E\left(R,t\right)=\frac{1}{n}{\displaystyle {\sum }_{i=1}^n{\Vert q_i-\left(R{p}_i+t\right)\Vert }^2}$ (6)

其中R为旋转变换矩阵，t为平移向量。目标是找到合适的R和t值，使误差函数最小。

3) 对源点云数据集进行更新 ${p^{\prime }}_i=R{p}_i+t$ ，R与t为上步所得的矩阵与向量。

4) 计算新的变换点集 $p^{\prime }$ 与q_i的平均距离d

$d=\frac{1}{n}{\displaystyle {\sum }_{i=1}^n{\Vert {p^{\prime }}_i-q_i\Vert }^2}$ (7)

5) 给定阈值，若平均距离d小于阈值，则迭代结束，否则继续迭代直到满足收敛。

4. 实验结果与分析

实验采用的点云数据为斯坦福bunny数据。实验流程如图2。选取bun000为源点云，bun045为目标点云，原始点云如图3所示，图4为采用传统ICP算法的配准结果。接下来采用改进的ICP配准方法。在实验中参数设置为：最大迭代次数为50次，两次变换矩阵的差值为10⁻⁸，均方误差为0.1。从图4和图5的对比来看，改进算法在精配准前对点云的初始位姿进行了优化，获得了更好的配准效果。表1为本文算法与传统ICP算法的比较，从表中数据可知，改进算法从精度和效率上都比传统ICP算法有了一定的提高。

Figure 2. Experimental algorithm flow

图2. 实验算法流程

Figure 3. Original point cloud

图3. 原始点云

Figure 4. Traditional ICP registration results

图4. 传统ICP算法配准结果

Table 1. Comparison between improved algorithm and traditional ICP algorithm

表1. 改进算法与传统ICP算法的对比

Figure 5. Improved algorithm registration results

图5. 改进算法配准结果

5. 结论

本文在传统的ICP算法的基础上进行了改进，将基于FPFH的采样一致性初始配准方法与ICP配准方法相结合，并通过PCL库加以实现，编程环境为VS2010。从实验的结果来看，本文的方法比传统ICP方法效果更好，精度和效率都更高。但是这种方法也有不足的地方，本文的方法只是让ICP算法有了一个比较好的初始位置，而在精配准算法方面沿用了ICP的传统算法，没有利用有效的方法去除错误点对，这对配准精度有一定的影响。因此在今后的研究中，找到合适的方法去除错误点对并进一步改进ICP算法是努力的方向。

基金项目

重庆市教委科学技术研究项目(项目编号：KJQN201800747)。