旋转和平移双推测的ICP配准加速算法

罗方燕，罗杰红，熊建斌

（1.广东职业技术学院，广东佛山528041; 2.广东石油化工学院，广东茂名525000）

旋转和平移双推测的ICP配准加速算法

罗方燕1，罗杰红1，熊建斌2

（1.广东职业技术学院，广东佛山528041; 2.广东石油化工学院，广东茂名525000）

为了提高迭代最近点(Iterative Closest Point,ICP)的运算速度，在原始ICP及其加速算法(Accelerated ICP,AccICP)的基础上，提出旋转和平移配准参数双推测的加速算法。该算法通过对配准参数进行统一推测的AccICP进行分析，针对配准参数迭代求取过程中旋转和平移参数存在变化不同步的情况，提出了对该俩参数进行独立推测，只要任一参数迭代变化符合要求即可进行推测。实验结果表明，该算法比原始ICP具有较好的加速效果；与AccICP算法相比，做到了精准推测，减少了不必要的统一推测。

旋转和平移双推测;迭代最近点算法;点云配准;加速算法

一、引言

三维点云数据拼接技术是计算机三维测量领域的研究热点和难点，其中一个关键步骤是点云配准，需要将不同视角下采集的三维点云数据拼接在一起。点云拼接技术可以分为粗拼接和精确拼接两个步骤[1-3]，粗拼接技术主要包括转台法、标签法和曲面特征法等。精确拼接技术主要包括迭代最近点算法(Iterative Closest Point ICP)[4]，有向距离场方法(Signed Distance Fields)[5]，和遗传算法(Genetic Algorithms)[6]等,其中最典型的是Besl提出的ICP算法，自提出以来，有大量的学者专家对该算法进行改进和应用研究[2,3,7-17]。

Granger等[15]提出EM-ICP改进算法，将最大期望算法(Expectation Maximization Algorithm)应用于ICP算法中，可以精确地配准大规模点云数据。在该算法中使用马氏距离(Mahalanobis distance)代替标准ICP算法中的配准误差公式，然后使用最大期望算法进行求解。Chetverikovd等[17]提出了一种基于LTS(Least Trimmed Squares)的Tri-ICP算法，该算法可以配准部分重叠的点云数据。Mohammadzade等[16]提出一种基于点的法向量的ICNP算法，并很好地应用于人脸识别方面。Bouaziz等[13]提出一种Sparse-ICP算法用于适应在出现噪声和缺乏配准数据的情况，提高了ICP算法的鲁棒性，并且不需要预先设定初始位置。在该算法中，提出了与标准ICP算法不同的配准误差公式，在标准ICP中采用L2范数的平方，而在该稀疏ICP算法中采用L2范数的p次方，其中p∈[0,1]。

ICP算法的运行效率是研究的重点方面[18-22]，比如对实时要求比较高的深度视觉机器人中，要求对三维数据进行快速的配准，以便获得全局环境数据。Besl[4]对ICP提出了一种基于配准参数推测的加速算法(Accelerated ICP algorithm,AccICP)，在该算法中对参数的7个变量进行统一推测，

本文提出将旋转参数的推测和水平位移参数的推测分别单独进行，参数推测是否进行，主要取决于各自最新参数的变化情况。如果最新所求取的参数状态符合要求，则进行相应的参数推测。通过实验验证，该算法不仅能对ICP进行加速，而且与AccICP相比较，有很明显的加速效果。

二、迭代最近点算法简述

迭代最近点算法（ICP）[4]主要步骤如下：

给定两个从不同视角下扫描测量得到的数据集X和B，其中X为点集{x}i=1Nx，点的数量为Nx；数据集B的数据形式可以为点、线或三角面等不同方式。

ICP算法的核心是循环执行下面的计算，首先设定初始化：设循环次数k=0，待配准点云初始状态为Ak=X，配准参数向量，点云数据集X和B之间的距离均方差dk=0。

1）求取数据集B中到最新待配准数据集Ak中每一个点的最近点，并将这些最近点集合为点云，并记为Yk=C(Ak,B)；

2）使用四元数法，求取Yk和B之间的旋转矩阵和平移矩阵对应的配准参数向量

5）如果｜dk-dk-1｜＜τ，计算结果误差变化小于一定小的τ，则结束计算；否则继续迭代，返回步骤1。

三、AccICP算法分析

在ICP迭代计算过程中，前面阶段主要进行比较大幅度的调整，而在后面阶段主要进行微细的调整，与前面阶段相比，调整的方向和幅度是有规律可循的。在AccIPC算法[4]中，当相邻3个配准参数的夹角小于一定阀值时，可以根据最新3次的配准参数推测产生新的配准参数，若能带来更好的收敛效果，则采用，否则弃之。该加速算法对ICP具有较好的加速效果，但同时也存在一个问题，因为配准参数由7个变量组成，其中前4个为旋转参数，后3个为平移参数，进行参数推测时，同时作用于旋转和平移参数。但在实际的配准过程中，可能出现平移参数已经到位，旋转参数未到位，或旋转参数已经到位，但平移参数未到位的情况。

四、最近点迭代双推测加速算法

ICP在迭代过程中，求取的配准参数中包含旋转参数和平移参数，不但这两个参数求取的过程不相同，而且作用的范围也不一样，本算法中提出将这两个参数进行分别独立的推测，具体改进如下。

在原始ICP算法的基础上，当执行完步骤4时，执行以下操作：

4.3）同理确定平移参数的预测尺度和方向。

4.4）将所预测的配准参数作用于数据X，如果配准误差小于原来的误差，则采纳该配准参数的预测。

然后执行原始ICP算法的步骤5。

五、验证结果及分析

为了验证该加速算法，采用斯坦福计算机图形学实验室的3D扫描数据库[23]三个不同数量级别的模型，如图1所示，其中Bunny模型有1889个点，Dragon模型有5205个点，Happy模型有7108个点。首先对原模型给予一定角度和位置的偏移产生新的模型，然后通过原始ICP、AccICP和本文所提出的加速ICP算法对新-原模型对进行配准。根据配准实验结果，主要从配准误差、迭代次数和执行时间等方面进行分析。

图1 斯坦福计算机图形学实验室3D数据模型

图2和表1-3为数据模型Bunny、Dragon和Happy实验的验证结果。Bunny是属于验证数据量比较少的代表，由图2(a)和表1可以发现，在该模型中，原始ICP算法进行了26次迭代，AccICP算法和本文的算法同样迭代了17次。在图2(a)的子图收敛趋势图中，可以比较清楚的看到，从第8次的迭代开始AccICP算法开始起加快作用了收敛速度，而本文所提出的算法从4次开始就起加速作用了，比旋转和平移参数同时预测的AccICP算法提前了4次迭代。

从表1和图2(a)中，算法的配准精度达到了原始ICP和AccICP算法的精度，同时执行时间比原始ICP算法稍少。在Bunny模型验证中，AccICP算法进行了8次预测，而本文算法进行了旋转预测7次，平移预测9次，包括旋转和平移同时预测的有5次，总共预测了12次，比AccICP算法多了4次。通过本文算法进行了2次单独的旋转预测和4次单独的平移预测，即在另一个方面不符合条件时，仍可以进行预测，加速迭代算法的收敛性。

图2 不同ICP算法的收敛性比较

图2(b)和表2是Dragon模型的验证结果，三个算法中的计算误差都符合计算要求。在原始ICP算法中总共进行了43次迭代，耗时为2.581秒，由于数据量比较大，比Bunny模型耗费时间多。AccICP和本文的算法都起到加速的作用，在该模型中，本文的算法比AccICP算法起到更好的加速作用。在AccICP算法中，从第18次开始加速，累计进行了9次有效预测加速，总共比原始ICP算法少了10次迭代。而在本文算法中，从第7次就开始加速，通过进一步的预测，加速的速度比AccICP快，分别进行了10次旋转和平移加速，其中9次是同时进行的，最后迭代23次后达到收敛的效果。执行效率方面也略比AccICP快，达到了加速的效果。

表1 Bunny模型的验证结果

表2 Dragon模型的验证结果

表3 Happy模型的验证结果

图2(c)和表3是数据模型Happy的验证结果，该模型的数据量比前面2个验证稍大，在原始ICP算法中的迭代次数和运行时间都会比较大。首先在该模型验证中，本文的算法与其他算法同样都到了收敛效果，在运行效率方面，比原始ICP算法快。从图2(c)中可以发现，AccICP算法从第6次迭代开始加速，而文中算法从第5次就开始了加速，虽然加速的效果与AccICP相当，但与原始ICP算法48次的迭代相比较，加速后才执行了22次的迭代，加快了算法的运算速度。同时从表3可以看到，AccICP算法进行了14次的推测，而文中算法进行了旋转推测12次，平移推测9次，其中同时执行的推测有8次。

从上述三维数据的验证结果，文中算法达到了预期效果，与AccICP相比，起到了再次加速效果。在AccICP算法中，判断的依据是当相邻3个七元配置参数的变化情况比较稳定时，进行线性或二次抛物线的推测，在这过程中需要这7个配置参数同时符合要求，并且同时预测。该算法不单推测的条件苛刻，而且推测后的效果也不一定符合收敛要求。

将七元配置参数分为两种情况进行分析推测，一是四元旋转参数符合要求时，另一种情况是三元平移参数。如果旋转参数符合推测的条件，则进行旋转推测，同理平移参数符合推测条件时，进行旋转推测。两种推测互不冲突，可以同时进行。

六、结论

本文就对经典的迭代最近点算法(ICP)及其改进的加速算法(AccICP)进行分析，在研究AccICP算法的基础上，提出了基于旋转参数和平移参数独立推测新的加速迭代最近算法。该算法以ICP为基础，在完成每次迭代后，对旋转参数和平移参数进行单独的推测判断和应用，如果推测结果能达到更好的收敛效果，则采用推测结果。

文中使用斯坦福大学3D数据库作为载体进行验证，通过对结果进行，新的算法起到了有效的加速效果，其主要特点如下：

1)算法保证了原ICP算法的单调收敛性，确保迭代步骤之后，两三维数据的距离越来越近。

2)与AccICP算法相比较，新的算法起到进一步加速的作用，加快了算法的收敛。

3)由于新算法是基于原ICP算法的一种加速改进，仍然是一种局部最优化算法，在配准前需要较好地初次配置，否则不能达到配准效果。

4)该加速算法可以应用于其他ICP改进算法中，提高运行速度。

[1]Salvi J,Matabosch C,Fofi D,et al.A review of recent range image registration methods with accuracy evaluation[J].Image and Vision Computing,2007,25(5):578-596.

[2]Tam G,Cheng Z-Q,Lai Y-K,et al.Registration of 3d point clouds and meshes:A survey from rigid to non-rigid [J].Visualization and Computer Graphics,IEEE Transactions on,2013,19(7):1199-1217.

[3]Santamaría J,Cordón O,Damas S.A comparative study of state-of-the-art evolutionary image registration methods for 3d modeling[J].Computer Vision and Image Understanding, 2011,115(9):1340-1354.

[4]Besl PJ,McKay ND.A method for registration of 3-d shapes[J].Pattern Analysis and Machine Intelligence,IEEE Transactions on,1992:586-606.

[5]Masuda T.Generation of geometric model by registration and integration of multiple range images[C].in 3-D Digital Imaging and Modeling,2001.Proceedings.Third International Conference on.2001.IEEE.

[6]Chow CK,Tsui HT,Lee T.Surface registration using a dynamic genetic algorithm[J].Pattern Recognition,2004,37(1): 105-117.

[7]严剑锋,邓喀中.基于特征点提前和匹配的点云配准算法[J].测绘通报,2013,（9）:62-65.

[8]戴玉成,章爱武.三维激光扫描数据快速配准算法研究[J].测绘通报,2010,（6）:8-11.

[9]Korn M,Holzkothen M,Pauli J.Color supported generalized-icp[C].in International Confer-ence on Computer Vision Theory and Applications.2014.

[10]Dong J,Peng Y,Ying S,et al.Lietricp:An improvement of trimmed iterative closest point algorithm[J].Neuro computing, 2014.

[11]Yang J,Li H,Jia Y.Go-icp:Solving 3d registration efficiently and globally optimally[C].in International Conference on Computer Vision.2013.

[12]Ma J,Zhao J,Tian J,et al.Robust estimation of nonrigid transformation for point set registration[C].in Proceedings of IEEE conference on Computer Vision and Pattern Recognition.2013.

[13]Bouaziz S,Tagliasacchi A,Pauly M.Sparse iterative closest point[C].in Computer Graphics Forum.2013.Wiley Online Library.

[14]Maier-Hein L,M.Franz A,Santos TRd,et al.Convergent iterative closest-point algorithm to accomodate anisotropic and inhomogenous localization error[C].in IEEE Transactions on Pattern Analysis and Machine Interlligence.2012.

[15]Granger S,Pennec X.Multi-scale em-icp:A fast and robust approach for surface registration,in Computer vision—eccv 20022002,Springer Berlin Heidelberg.p.418-432.

[16]Mohammadzade H,Hatzinakos D.Iterative closest normal point for 3d face recognition[J].Pattern Analysis and Machine Intelligence,IEEE Transactions on,2013,35(2):381-397.

[17]Chetverikov D,Svirko D,Stepanov D,et al.The trimmed iterative closest point algorithm[C].in Pattern Recognition, 2002.Proceedings.16th International Conference on.2002.

[18]Chin T-J,Bustos AP,Brown MS,et al.Fast rotation search for real-time interactive point cloud registration[C].in Proceedings of the 18th meeting of the ACM SIGGRAPH Symposium on Interactive3D Graphics and Games.2014.ACM.

[19]Yu W.Fast and robust image registration for 3d face modeling[C].in Information Processing,2009.APCIP 2009.Asia-Pacific Conference on.2009.IEEE.

[20]Parra Bustos AJ,Chin T-J,Suter D.Fast rotation search with stereographic projections for 3d registration[C].in Computer Vision and Pattern Recognition(CVPR),2014 IEEE Conference on.2014.IEEE.

[21]Wu F,Wang F,Jiang P,et al.A nearest neighbor searches(nns)algorithm for fast registration of 3d point clouds based on gpgpu[C].in 2015 International Conference on Intelligent Systems Research and Mechatronics Engineering.2015.Atlantis Press.

[22]Eckart B,Kim K,Troccoli A,et al.Mlmd:Maximum likelihood mixture decoupling for fast and accurate point cloud registration[C].in 3D Vision(3DV),2015 International Conference on.2015.IEEE.

[23]Turk G,Levoy M.Zippered polygon meshes from range images[C].in Proceedings of the 21st annual conference on Computer graphics and interactive techniques.1994.ACM.

（责任编辑：魏树峰）

Accelerated ICP Registration Algorithm Based on Expectation of Both Rotation and Translation

LUO Fang-yan1,LUO Jie-hong1,XIONG Jian-bin2
(1.Department of Information Engineering,Guangdong Polytechnic,Foshan 528041,China 2.School of Computer and Electronic Information,Guangdong University of Petrochemical Technology,Maoming 525000,China)

To increase the calculation speed of Iterative Closest Point(ICP)Algorithm,a new accelerated ICP is proposed based on the original ICP and its accelerated ICP(AccICP).Both rotation and translation registration parameters are expected in the algorithm.The whole registration parameters are expected in the AccICP,and the changes of both rotation and translation registration parameters may not be synchronized during the process of iterative calculation.Both parameters are expected respectively in the new proposed method,and it can do expectation independently if one parameter's change meets the requirement.The experimental results show that the proposed algorithm can accelerate calculation,compared with the original ICP.Compared with AccICP,it does accurate expectation,and avoids unnecessary unified expectation.

expectation of both rotation and translation;Iterative Closest Point;point cloud registration; accelerated algorithm

TP391

A

2016－08－20

罗方燕（1981-），女，广东梅州人，实验师，研究方向：三维数据处理。E-mail：532443807@qq.com.

国家自然科学基金项目(No.61271380)；广东省自然科学基金项目(No.2014A030307049)

1671－802X(2016)05－0006－06