基于多分辨率的快速迭代最近点配准算法

王 硕 王亚飞 李学华

(北京信息科技大学信息与通信工程学院 北京 100101)

0 引 言

点云配准是计算机视觉和图像处理的一项重要研究课题。该技术广泛应用于医学[1]、移动机器人导航[2]、计算机辅助设计[3]、三维重建[4]等领域中。通过特定的几何变换，点云配准旨在使两组不同图像的点达到空间一致性。换句话说，从参数未知的初始变换开始，它试图找到一个最优的变换，使两个不同相机坐标系下的点云转换到同一个世界坐标系下。

关于点云配准的研究可以追溯到20世纪80年代早期，当时专门针对二次曲面提出了全局刚性点集配准算法。1992年Besl等[5]提出了经典的ICP算法，用于点云的精确配准，后续ICP算法的改进主要围绕提高配准精度和加快配准速度两个方面进行研究。为了改进ICP算法的配准精度问题，Rusu等[6]提出快速特征直方图(FPFH)特征，目标是通过点周围的平均曲率来编码其邻域的几何属性，该算法能很好地应对邻域中存在不同采样密度或不同噪声水平的点，进而提升配准精度。Seal等[7]使用Hausdorff距离计算两点云间误差，降低收敛误差。Ye等[8]将点的颜色和深度信息一起作为配准因素，提高了配准精度，并避免初始值的影响。文献[9-12]提出了一种基于关键点的点云配准方法，根据关键点不变的特性实现精确配准，然而每个点的特征计算量很大，时间消耗较多。唐志荣等[13]，提出一种基于多维混合柯西分布的点云配准算法，该算法将点云数据模型转换为多维混合柯西分布模型，用数据中心点进行配准，该算法配准效果较好，但配准效率有待提升。

以上方法由于要计算点云更多维度的特征，增加了点云配准时间。为加快配准速度，钟莹等[14]使用欧氏距离阈值和法矢量夹角阈值来选取正确的匹配点对，这种方法可降低点云间点对的噪声带来的干扰，减少迭代次数，但该算法过于依赖于阈值的选定，阈值设置不佳会导致精度的下降。杨秋翔等[15]根据每一点的法矢量角计算该点的关键度，依据关键度对点云数据进行精简，提高了配准效率，但当点云中点数超过十万时，关键度的计算成为主要的耗时部分。Ji等[16]利用遗传算法将点云转化为三角网搜寻最近点的方法实现点云配准，具有较高的配准效率，但配准精度有待提升。王畅等[17]使用全局结构特征做初始配准，之后利用局部结构特征做快速精确配准算法。该算法提高了配准速度，但当点云数较少的情况下，可能会使算法配准结果不理想。赵敏等[18]提出了一种基于lp空间力学模型的点云配准算法，其中：l表示线性赋范空间，p表示空间维度。该算法收敛速度快，但当物体形状结构接近对称时，会降低配准精度。陈旭等[19]提出一种基于校正点云主成分方向的线性计算方法，配合ICP算法实现高效精确的点云全局配准，但此算法在点云分布不均匀或有缺陷时结果可能有所偏差。传统ICP算法的时间复杂度为O(mdnd)，m、n分别为d维空间里点的数量，即ICP算法的计算效率受点集数量和空间维数的影响[20]。

本文主要针对ICP算法配准效率问题进行研究，提出一种易于实现且鲁棒性高的多分辨率ICP配准算法。首先使用本文提出的自适应体素网格滤波器对点云进行多分辨率采样，然后使用提出的多分辨率思想，从低分辨率到高分辨率进行配准，利用低分辨率点云进行快速初始配准，再利用高分辨率点云提高配准精度，完成粗略到精细的匹配。实验结果表明，本文算法简单可行，改善了ICP算法计算效率低的问题。

1 传统ICP算法

由Besl和McKay所提出的传统ICP算法，是一种高效处理两个点云间配准的方法。通常，刚性配准是确定两个点云的点集之间的刚性变换的过程，可以匹配对应点。给定两组3D点云：待配准点云P={p(1),p(2),…,p(n)}和目标点云Q={q(1),q(2),…,q(m)}，其中n和m分别表示两组点云中点的数量。ICP算法的目标就是找到旋转变换矩阵R和平移变换矩阵t，使得∀i,q(i)=Rp(i)+t。然而由于实际配准过程中，存在噪声、误差、拍摄角度等问题的影响导致上式不会完美成立，即不存在一个R和t可以使所有点满足上式。因此ICP算法的目标转换为求旋转R和平移t，使两点云间对应点距离之和最小，目标函数f(R,t)表示为：

(1)

ICP算法以迭代的方式寻找最优值，其基本步骤如下：

1) 寻找待配准点云在目标点云中的对应点。根据待配准点云P的点坐标，在目标点云Q中搜索相应的最近点点集。

2) 根据点云匹配点对，求使对应点间平均距离最小的刚体变换参数。k对点可以得到k个方程组，使用最小二乘法求解出方程中的参数R和t。

3) 应用变换并计算新的对应点间距离。对点云P中每一个点应用转换公式Pj=RPj+t，其中下标j表示第j轮迭代。之后重新计算新的对应点间距离Dj：

(2)

4) 判断是否进行下一轮迭代。设定精度阈值σ和迭代最大次数N，若步骤3中求出的新的对应点间距离Dj小于阈值σ或迭代次数超过N，则停止迭代，否则重复步骤1-步骤3继续迭代。

在ICP算法配准过程中，两个点云里点的数量和迭代次数对计算时间有很大影响。传统ICP算法中将所有点都纳入到计算过程中，严重影响了配准效率。本文提出一种基于多分辨率配准的ICP算法，以改善配准效率低的问题。

2 多分辨率配准的ICP算法

2.1 体素网格滤波器

对点云进行多分辨率采样时，应尽量保持原点云的曲面特征以及点的密度分布。若使用随机采样法，点云的轮廓信息将被削弱，并且会造成点分布不均匀。使用体素网格滤波器对原点云进行采样处理，该方法可在减少点云里点的数量的同时，保持点云原来形态特征，但是采样时体素立方体的边长需要人为指定。因此，本文提出自适应体素网格滤波器，根据点云中包含的点数自动计算体素网格的边长，可对不同规模的点云自动做出适合的多分辨率采样。

2.1.1传统体素网格滤波器

体素网格滤波器的基本原理是：将原始输入点云分成多个相同大小的立方体，立方体的大小可根据实际需求进行调整。然后计算每个立方体中所有点的重心，以重心替代这个立方体中的所有点。具体计算方法如下所示：

1) 确定立方体边长K，将点云按顺序分成多个大小为K×K×K的立方体。K值越大，采样后的点云的分辨率越低，反之，则分辨率越大。采样立方体个数为A×B×C，计算方法如下：

(3)

式中：xmax、xmin、ymax、ymin、zmax、zmin分别为X、Y、Z三个坐标轴上点坐标的最大值、最小值。

2) 计算每个立方体包含点的重心(x′,y′,z′)，计算方法如下：

(4)

式中：m为该立方体中点的数目。

3) 以重心代替该立方体中所有点，并将所有立方体的重心重新合成为新的点云。

2.1.2自适应体素网格滤波器

针对不同点数的点云，本文提出的体素网格滤波函数中立方体的边长自适应更新策略。

立方体边长K的计算如下：

(5)

式中：l表示第l层滤波，l=0时为原始分辨率；Dori为原始分辨率点间平均距离。其计算如下：

(6)

式中：N为点云里点的总数；Hi为点云中第i个点；Hdi为距离Hi最近的点。

式(5)中β(s)为自适应比例因子，控制体素网格立方体边长增长率，定义为：

(7)

2.2 改进的ICP算法

2.2.1整体思路

本文在ICP配准算法中引入多分辨率的思想。在ICP算法前几轮迭代的初始配准中，首先使用较低分辨率的点云，快速获得两个点云之间的初始变换，提高匹配效率。然后使用初始变换的结果，以更高的分辨率实现更精确的配准。不同分辨率中点云的形状基本保持一致，只有点云中点的数量不同，因此较低分辨率的配准结果，可作为高分辨率配准的初值。点云里点的数量越少，算法的计算速度越快，同时精度会有所下降；点的数量越多，匹配的精度会提升，但计算耗时会大幅增加。使用较低分辨率的两个点云获得的配准结果，比用原始分辨率获得相同精度的配准结果的耗时显著减少。该算法的优点是使用快速低分辨率配准代替高分辨率配准的初始迭代过程，减少了高分辨率下的配准迭代次数，从而提高计算效率。所提出的快速多分辨率ICP算法流程图如图1所示。

图1 本文算法流程图

2.2.2详细流程

本文提出的多分辨率配准详细流程如下：

1) 使用提出的自适应体素网格滤波器对两点集P、Q进行多次不同分辨率采样。

Hl=(Hl-1)l=1,2,…,L

(8)

(9)

(10)

3) 重复步骤2)直到dl不再减小为止。

4) 令l=l-1提升点云分辨率，得到Pl-1和Ql-1。

5) 重复步骤2)-步骤4)，直到l=0或均方根误差(RMS)达到设定阈值为止。RMS计算方法如下：

(11)

3 实验结果与分析

为了验证所提出的方法的鲁棒性和计算效率，分别与传统ICP算法、文献[14]、文献[15]、文献[18]和文献[19]改进的ICP算法的配准时间和精度进行了仿真和比较。实验使用斯坦福大学开放点云数据库的兔和龙模型进行对比试验。实验平台为Ubuntu 16.04系统，CLion 2016，运行在配置为Core i7-8700K CPU 3.8 GHz，16 GB RAM的台式计算机上。

实验中待配准点云模型初始位置如图2所示，白色为目标点云、浅灰色为待配准点云。其中兔模型点云中点的数量为35 947，龙模型点的数量为437 645。

图3和图4分别为兔和龙模型在不同分辨率下的点云模型。采样后点云最低包含点数设为1 000。由式⑸计算可得，兔模型点云下采样计算1次，体素网格边长K=0.005；龙模型点云下采样计算2次，体素网格边长K分别为0.001和0.005。表1为兔和龙模型在不同分辨率下的采样点数和采样耗时。可以看出使用体素网格滤波器采样得到的不同分辨率的点云模型在降低点云点数后，仍保留了原始点云的形态特征，且由多分辨率采样带来的计算耗时极短。

(a) 兔模型 (b) 龙模型图2 待配准点云

(a) K=0.005 (b) 原始分辨率图3 兔模型多分辨率采样结果

(a) K=0.005 (b) K=0.001

(c) 原始分辨率图4 龙模型多分辨率采样结果

表1 多分辨率采样计算耗时

图5为兔和龙模型配准实验结果。可以看出传统ICP、文献[15]、文献[18]、文献[19]和本文算法都可以实现点云的精确配准，配准结果基本一致，从视觉上都可以认为点云正确配准；文献[14]算法效果略差，出现了轻微重影的现象。

(a) 原始ICP配准结果

(b) 文献[14]配准结果

(c) 文献[15]配准结果

(d) 文献[18]配准结果

(e) 文献[19]配准结果

(f) 本文改进的ICP配准结果图5 兔子及龙点云模型配准结果

表2为四种算法的配准误差、迭代次数和运行时间的比较。配准误差(RMS)停止阈值为1×10-4m。该结果表明在配准精度基本相同的情况下，本文算法相对于传统ICP、文献[14]、文献[15]和文献[18]算法大大缩短了配准时间。尤其是在龙模型配准实验中，由于点数过多导致传统ICP算法耗时成指数级增长，远超配准时长所能接受的范围，文献[14]、文献[15]的算法虽比原始ICP算法耗时降低一半左右，但时间仍很长。文献[19]的算法已将耗时降低至可接受的范围内，但本文改进的ICP算法将配准时间进一步缩短。

表2 算法性能比较

六种算法在兔模型上的配准过程如图6所示。可以看出，本文算法配准耗时远低于传统ICP算法。在基本相同的配准精度下，传统ICP需要近2.4 s；文献[14]、文献[15]的算法需近1.5 s；文献[19]显著降低配准时间；而本文算法可在前几轮迭代中完成快速初始配准，在后几轮迭代中对配准精度进一步提高，总时长控制在0.5 s内，耗时降低为原始ICP算法的20%左右，且比文献[19]算法耗时更少。

图6 兔模型配准时间比较

为进一步验证本文算法在效率上的提升，选取了另外5组不同点数的点云进行配准实验。配准误差(RMS)停止阈值仍为1×10-4m，在各算法配准精度均到达这个标准后，将本文算法的配准耗时与传统ICP、文献[15]和文献[19]的耗时进行对比，如表3所示。可以看出，本文算法相比传统ICP算法，配准耗时仅为原来的15%及以下；相比文献[19]的算法，在各个模型中，本文算法耗时均低于文献[19]算法的耗时，且随着点云规模的增大，配准所用时间降低的越多。实验结果表明，本文提出的改进的ICP配准算法可以快速准确收敛。

表3 不同规模点云配准耗时 s

4 结 语

本文提出了一种多分辨率改进的ICP快速匹配算法用于两点云间配准。该算法构造了一种用于点云匹配的多分辨率模型，并使用提出的自适应体素网格滤波器对点云进行多分辨率采样。可以在低分辨率下快速获得两个点云之间的初始变换，然后使用先前变换以更高分辨率进行更精确的配准。通过这种方法降低了ICP算法在初始配准过程中需要处理的点数，从而降低配准时间。实验表明，本文算法提高了ICP算法的配准效率，且随点云规模增加，速度提升幅度递增趋势。