应用改进ICP算法的点云配准

杨小青，杨秋翔，杨 剑，郑晓璐

（中北大学 计算机与控制工程学院，山西 太原030051）

0 引 言

随着计算机辅助设计技术的提高，利用实物数字化技术采集数据的逆向工程技术得到越来越广泛的应用［1］。然而在实际测量中由于受到测量仪器和环境的影响，需要多次测量才能获得物体表面的完整信息，在后期点云数据配准中可能存在一定程度上的旋转、平移错位等问题［2］。因此，提出一种改进策略优化ICP 算法，对多次测量所得的覆盖物体局部信息的点云数据进行有效整合和配准具有非常重要的意义。

点云配准中应用最广泛的是迭代最近点 （iterative closest point，ICP）算法及其改进算法［3－5］，决定ICP 算法 效率和精度的关键在于能否在海量点云数据中，正确、快速地完成基准表面与待匹配表面距离最近点的搜寻。对于最近点的选取，常用方法有多邻接三角面距离最近点法［6］、二次曲面逼近法［7］、度量点k 邻域特征空间相似度最大法［8］，上述方法虽能有效加快算法运行速度，但其鲁棒性较差。对于传统ICP 算法存在的收敛速度慢、易于陷入局部最优化等问题，张晓娟等提出根据点云拓扑关系中各点周围的8个邻近点，按照一定邻近关系对应准则在邻近候选点中搜索最近点的算法思想［9］，该方法改善了传统ICP算法存在的问题，但匹配耗时较长，影响算法效率。

本文针对此问题，利用盒子结构划分点云数据，将最邻近点的查找控制在较小的空间范围中，针对每一独立单元盒提取特征点构建三角形，结合相似原理，计算最近点对之间的匹配度及其对其余点对匹配正确性的支持度，最终提出一种点云配准改进ICP算法。

1 ICP算法原理与步骤

1.1 基本原理

ICP算法对待拼接的两片点云要求有较大范围的重合区域，首先按照一定准则确定对应点集P 和Q，利用最小二乘法的优化思想，重复选择对应点对，并计算最优刚体变换将不同坐标系下点云数据合并到同一坐标系中，直到可以满足精确配准的收敛精度要求。通过计算求得使目标函数 （1）最小化的旋转矩阵R 和平移矢量T，从而得到两片点云之间的配准矩阵

式中：Pi——源数据点集，Qi——目标点集中对应Pi的最近点，R——3＊3矩阵，T——3＊1矢量，F（R，T）——源点集经过平移和旋转后，其点集中每个点与目标点集中对应点之间距离的平方和，要满足最小二乘的要求，即要使得F（R，T）达到最小。

1.2 算法步骤

ICP算法是要找到源数据点云与目标数据点云之间的R和T，使得两数据点集之间满足某种度量准则达到最优匹配。ICP配准步骤如下：

（1）对原始点云数据进行采样。尽可能的保留表针物体关键特征的数据点，减少噪声点，从而提高后续步骤的效率；

（2）确立初始对应点集。在两点云数据之间以距离最近点来近似替代真实对应点确定对应关系；

（3）去除错误对应点对。引入某一种约束条件或者评价标准用以去除不可靠、不兼容的错误点对；

（4）求解坐标变换。ICP 算法应用最小二乘法的思想迭代求解两片点云之间的最优坐标变换。

2 ICP算法的改进

依照ICP算法的实现步骤可以看出，在原始数据点云与待配准点云之间搜寻最近点对是整个算法的关键环节，也是最终快速、准确地得到配准结果的有力保证，传统ICP算法在点云重叠区域内搜寻最近点的时间复杂度为O（Ai×Bj），假设Ai、Bj是两点云各自的点集数目。显然倘若可以有效改进寻找最近点的策略，算法效率将会得到非常大的提升，以此作为算法改进的出发点，引入盒子结构划分点云，缩小最近点的查找范围，按照相似原理对最近点的选取加以约束，计算其支持度保证对应点对的查找正确性。具体如下：

2.1 盒子结构

盒子结构的建立过程分为两步：

（1）按照初始尺度x 对点集进行分割。以平行于X、Y、Z轴的棱边为基准，完全包围点云的最小长方体为盒子外围，以1mm 的宽度根据每个点的坐标位置进行空间区域的等间隔分割，从而得到许多大小相等 （Mx×Mx×Mx）的小立方体。依次计算每个单元盒子内点的数目，如果大于50个点，要求对该盒子进行更细致的均等分割；

（2）对盒子结构进行排序。根据盒子划分参数和每一点的坐标确定各点所在立方体的位置编号，先依次比较每个单元盒顶点坐标最小的点的Z 坐标，升序排序，对于Z坐标相同的，则比较Y 坐标，同理，对于Z坐标、Y 坐标均相同的，按其X 坐标大小重排点云。

容易看出，对海量点云数据进行盒子结构划分且经过排序之后，最近点的查找范围将更加明确，为大幅度提升对应点对的查找速度提供较大可能性。

2.2 相似原理

对于任一三角形，在经过比例放大、旋转、平移变换之后与原三角形相似，将这一特性引入到点云匹配算法中，设法在源数据点云与目标点云中分别构建三角形，利用相似关系，确定点对对应关系。

假设在源数据点集P中找到3个特征点pi、ph、pu在目标点集Q 中的对应点分别是qj、qk、qv，通过上述原理可知，不管是否存在旋转与平移比例变化，由pi、ph、pu三点构建的三角形与由qj、qk、qv组成的三角形相似。两三角形的各边分别表示如下

由相似原理，有下列对应关系

2.3 改进算法步骤

按照上述盒子结构的分割思想将源点云数据进行盒子结构的划分，并重新排序。之后，以每一盒子为独立分析单元，计算当前有限点云数目的盒子内各点的曲率值进行特征点的选取。其选取原则是：首先设定一个点云数据曲面的平均曲率阈值，依次计算每个点的平均曲率，并将平均曲率大于预设阈值的点作为候选特征点，然后计算各候选特征点的曲率，将求得的曲率极值点作为真正特征点。

在源数据点集P已提取出的特征点中任选不共线的三点pi、ph、pu构建三角形，任选两点，譬如为pi、ph，首先找到其在目标点集Q 中的最近点分别为qj、qk。而对于另一点pu对应点的确定，则利用相似原理对其进行约束筛选，即在目标模型点云数据中找到一点qv，使得由qj、qk、qv组成的三角形与源点云数据中pi、ph、pu构建的三角形满足相似性，并将qv作为pu在目标点集中的初始对应点。对于每一盒子结构单元内的其它点可以按照上述思想依次寻找其最近点，即以已选定三角形的任一条边为基准，继续找到该直线外某一点构成新的三角形，仍然对其进行相似性约束，如此循环往复下去，直到所有的源数据点云与目标数据点云的对应点对关系都被确定。

对于每 一 六 元 组 （pi、ph、pu、qj、qk、qv），定 义 两 三 角形的相似度。令δjhk（u，v）为当pi与qj匹配且ph与qk匹配时，由 （pi、ph、pu）组成的三角形与由 （qj、qk、qv）组成的三角形的相似度。令

则

若δjhk（u，v）为0，表示两三角形完全相似，说明pu相对于pi、ph等同于qv相对于qj、qk，即点对（pu，qv）已经给予（pi，qj，ph，qk）最大的支持度。随着δjhk（u，v）不断增加，支持度减小。因此，由相似度可得到支持度的定义如下

使得当pi与qj配对、ph与qk配对时，pu只与和其相互联系的、对（pi，qj，ph，qk）有着最大支持度的qv相匹配，此时可以确定 （pu，qv）为真正对应点对，从而保证利用相似原理约束后查找到的对应点对关系的正确性，即有

所有pu求和后 取平 均值，得到（pi，qj，ph，qk）的初始匹配度量值表示为

以及（pi，qj）的初始匹配度量值如下

进行第r次 （r＞0）迭代时，（pu，qv）对于（pi，qj，ph，qk）的匹配度量值依赖于pu、qv之间的位置差别及其Sr－1（pu，qv）的值。为体现出这两个因素之间的相互作用，取二者中的最小值，则有

以及

为避免由于门限的选取不当带来的问题，取

预先设定一个极小正数ε为判定阈值 （如0.000001），则当dr＜ε时，迭代终止。

综上算法步骤可以看出，与传统ICP 算法寻找对应点相比较，改进后的算法将两片点云数据中对应点的查找加以盒子结构和相似原理下要求支持度达到最大的匹配条件的双重约束，使得对应点的搜索速度大幅度提高，且出现错误匹配的概率很小。

3 实验结果与分析

为了检验本文提出的改进ICP算法的有效性，在Windows 7 64位操作系统上，基于VS2010上的VC＋＋平台，采用PCL1.6.0 版 本 点 云 库［10］、CMAKE 3.0.0 跨 平 台 编译工具，搭载ASUS Xtion RRO LIVE体感摄像机，选择与其兼容的OPENNI第三方库作为点云数据的输入源，进行点云数据的配准实验。实验所用测试数据来源于PCL 点云库官网［11］。

在CMAKE中建立工程文件，生成相应的可执行文件并运行。将点云数据转换为PCD 文件格式，确定输入的待匹配点云数据量大小，分别用不同颜色表示源点云、目标点云，并建立不同视口的可视化对象显示未配准和配准后的对比效果。经过多次实验效果对比，当手动迭代次数设置为10次时，其配准效果最好，精度最高，每迭代一次，在配准结果视口中刷新显示最新迭代效果，在算法运行迭代次数小于预设次数之前，不断刷新最新的配准结果，直至收敛。

利用传统ICP算法、文献 ［9］提出的邻近关系对应准则配准算法以及本文算法分别对Michael人物模型点云测试数据进行配准实验。获取到待配准的两片点云数据量分别为：5215和4918。实验结果如图1所示，其中图1 （a）表示待配准的两片点云，图1 （b）为经典ICP 算法配准后的结果，容易看出，传统点云数据配准算法远远达不到良好的匹配精度，不能满足实际配准要求。由图1 （c）可知，作为改进算法，文献 ［9］提出的某算法在一定程度上提高了配准精度，取得了较好配准效果，然而对比配准结果明显看到图1 （d）的配准精度和效果得到了进一步的改善，对实验所用点云模型中人物的头部，胳膊，腿部以及双脚等一些线条较复杂的细节区域的配准效果更为完整和精确，即应用本文提出的改进ICP算法可以大幅度提高点云配准效率。

本文提出的改进算法不仅在配准精确度上有较好体现，在配准速度方面也取得令人满意的实验结果，对照比较最原始的ICP算法以及文献 ［9］提出的改进算法，其在海量点云数据的匹配效率最高。为判别算法的配准误差，现定义误差参数μ，则有

图1 Michael点云数据配准效果

其中，H（p1i，q2i）表示经过精确配准之后两片点云中各对应点对（p1i，q2i）之间的距离，τ为设定的精度阈值，N 为点云数据点数，S（p1i，q2i）表示当前点对已达到配准精度要求，则μ表示点云中未达配准精度要求的数据点所占比率。

如表1所示上述3 种算法实现Michael点云配准过程中，在配准速度和精度方面的对比体现。分析可知，文献［9］算法在找到良好匹配初值情况下，缩小了配准误差，但就匹配速度而言，其所需配准时间较传统ICP 算法减小甚微；而本文提出的改进算法不仅大幅缩短配准时间，而且将配准误差缩减到0.03%。由此可见，通过对待配准点云数据划分盒子结构并排序，缩小对应点对的查找范围，提高了配准速度，同时引入三角形相似原理，验证当前查找到的对应点对对其余匹配点对的支持度是否为最大，从而保证对应点对的查找正确性，进而减小算法配准误差。

表1 点云配准比较

运用本文算法对另一组点云测试数据Wolf进行配准，其中图2 （a）为待配准的且有较大重合区域的两片点云，图2 （b）为配准后结果，配准后所得Wolf模型的头部、前后脚以及尾巴部分均显示出较明显的轮廓特征，可以看出同样能达到较好的匹配效果，如图2所示。

图2 改进ICP算法Wolf点云配准

如表2所示对于不同数据量规模的点云数据，采用上述3种算法进行配准实验的结果对比，由表可见，在不同规模的点云数据量下，文献 ［9］算法的效率优于传统ICP算法，而本文的改进算法效率则远远优于以上两种算法。

表2 不同规模的点云数据配准对比

按照表2中数据，在点云配准中本文算法效率与传统ICP、文献 ［9］算法效率相比较所提高的百分比。可以看出，较传统ICP算法效率提高了65%以上，较文献 ［9］算法效率提高了60%以上，而且随着点云数据量的增大，本文算法体现出的优势愈加明显，具体如图3所示。

图3 改进算法的效率提高比较

4 结束语

本文提出的对点云数据进行盒子结构划分并排序，在缩小对应点查找范围的基础上，引入相似原理对最近点的选择加以约束，通过计算已构成的对应点对，对其余两匹配点对的支持度是否达到最大来判断当前点对的查找正确性，从而保证算法的配准精度。依照上述实验结果和数据对比分析可知，改进后的算法在配准效率、配准精度上较传统算法都有大幅度地提高，尤其对于数据量庞大的带匹配点云模型，表现出的配准效率具有明显优势。

［1］Hacene A，Mekki A.Bio－CAD reverse engineering of freeform surfaces by planar contours［J］.Computer－Aid Design ＆Applications，2011，8 （1）：37－42.

［2］Senin N，Colosimo BM，Pacella M.Point set augmentation through fitting for enhanced ICP registration of point clouds in multisensor coordinate metrology ［J］.Robotics and Computer－Integrated Manufacturing，2013，29 （1）：39－52.

［3］DUN WZ，HUN G，HONG LX，et al.The research of optical 3D measuring precision influencing factor in reverse engineering ［J］. Applied Mechanics an Materials，2010，33：157－162.

［4］DU SY，ZHENG NN，YING SH，et al.Affine iterative closest point algorithm for point set registration ［J］.Pattern Recognition Letters，2010，31 （9）：791－799.

［5］LAURA EW，JOHN FP.A bounding box search algorithm for DEM simulation ［J］.Computer Physics Communications，2011，182 （2）：281－288.

［6］YUAN Jianying，LIU Xianyong，LIU Wei，et al.Research on precise registration of multiviews based on improved ICP algorithm ［J］.Transducer and Micro System Technologies，2008，27 （5）：27－30 （in Chinese）. ［袁建英，刘先勇，刘伟，等.改进ICP算法实现多视点云精确配准研究 ［J］.传感器与微系统，2008，27 （5）：27－30.］

［7］YANG Xianhui，WANG Huinan.Application research of ICP algorithm in 3Dpoint cloud alignment［J］.Computer Simulation，2010，27 （8）：235－238 （in Chinese）. ［杨现辉，王惠南.ICP算法在3D点云配准中的应用研究 ［J］.计算机仿真，2010，27 （8）：235－238.］

［8］WANG Rui，LI Junshan，LIU Lingxia，et al.Registration of point clouds based on gemotric properties ［J］.Journal East China University of Science and Technology （Natural Science Edition），2009，35 （5）：768－773 （in Chinese）. ［王蕊，李俊山，刘玲霞，等.基于几何特征的点云配准算法 ［J］.华东理工大学学报 （自然科学版），2009，35 （5）：768－773.］

［9］ZHANG Xiaojuan，LI Zhongke，WANG Xianze，et al.Research of 3Dpoint cloud data registration algorithms based on feature points and improved ICP ［J］.Transducer and Micro System Technologies，2012，3l（9）：116－118 （in Chinese）.［张晓娟，李忠科，王先泽，等.基于特征点和改进ICP 的三维点云数据配准算法 ［J］.传感器与微系统，2012，3l（9）：116－118.］

［10］ZHU Dehai，GUO Hao，SU Wei.Point cloud Library PCL tutorial［M］.Beijing：Beihang University Press，2012 （in Chinese）. ［朱德海，郭浩，苏伟.点云库PCL 学习教程［M］.北京：北京航空航天大学出版社，2012.］

［11］Point cloud library website.Point cloud data［EB／OL］.［2014－10－06］.http：／／www.pointclouds.org／（in Chinese）.［PCL点云库官网.点 云 数 据［EB／OL］.［2014－10－06］.http：／／www.pointclouds.org／.］