基于KDTree改进的ICP算法在点云配准中的应用研究

郭俊辉

（上海海事大学 信息工程学院，上海 201306）

0 引言

在地面三维激光扫描过程中，受物体尺寸、物体间的遮蔽以及扫描仪视场角等因素的影响，每站扫描只能获得本站扫描仪坐标系下的点云数据。对于大型立体模型而言，大多数情况下不太可能只通过一次扫描就获得全部的物体表面坐标及属性数据。因此，为了获得完整的物体表面坐标及属性数据，必须从不同的视角来扫描场景。在点云数据处理阶段，点云配准是十分关键的问题之一，配准的精细化程度直接影响后续操作。

1 扫描设备及作业流程

对物体进行点云数据采集时，采用的三维激光扫描仪是ScanStation C10全站式三维激光扫描仪。扫描参数设置情况如下：全景扫描，扫描视角为360°×270°，扫描速度为50 000点/s，扫描距离为300 m，点位标称精度为±2 mm。

设置好参数以后，分别在两位置坐标系下对只有4个面的长方体实物进行扫描采集。然后，依次进行点云数据的去噪、稀疏采样，由此获得能足够表达物体模型的点云数据。

图1为使用ScanStation C10扫描仪的作业流程，图2为经过去噪采样处理过的数据并可视化的结果。

图1 ScanStation C10扫描仪的作业流程

图2 两个坐标系下采集到的点云数据

2 配准定义

点云配准简单来说就是将从多个站点获得的点云数据进行拼接，得到一个统一坐标系下的三维数据点集。它类似于数学上的映射问题，也就是说要先找到两个点云数据集间的对应关系，然后将一个坐标系下的点云数据转换到另一个坐标系下。

配准过程主要有以下两个步骤：（1）寻找对应关系；（2）解算变换参数。即首先确定同名点对，然后解算旋转矩阵R和平移矩阵T。

同名点对：同一个点在不同坐标系下的表达。

图3所示为两站扫描示意图，在A、B两处分别安放扫描仪对同一个物体进行扫描。在A处获得坐标O1-x1y1z1下的点云数据M，在B处获得坐标系O2-x1y1z1下的点云数据N，配准的目的就是将两个坐标系O1-x1y1z1、O2-x1y1z1下的点云数据M和N转换到同一个坐标系下。

图3 两站扫描示意图

对于从两站采集到的点云集合 M和 N，Mi（X，Y，Z），Ni（x，y，z），且 Mi、Ni为 在 不 同 坐 标 系 下 的 同 一 点 ，严格来说，点云配准就是将全部来自两个不同坐标系下的同名点对（Mi，Ni）满足刚体变换（R，T），即：

其中，R为旋转矩阵，T为平移矩阵，α、β、γ表示沿 X、Y、Z 轴的旋转角，tx、ty、tz表示位移量。

式（1）称作空间相似变换公式，它是点云配准的基本公式。由式（1）可解出同名点转换参数，而后进行点云数据配准。

3 点云配准算法

目前，点云配准算法依据其采用的配准基元可将其分为无特征的配准和基于特征的配准[1]两大类。

基于特征的配准是指利用角点、边缘、面等几何特征[2]来解算变化参数。这类算法主要有以下几种：基于控制点的配准算法[3]、基于线特征的配准算法[4]以及基于曲率[5]的点云配准算法。

无特征的配准就是直接利用原始数据进行配准。此类算法中最为著名的是ICP （Iterative ClosestPoint）算法[6]，但该算法只适用于存在明确对应关系的点集，并且计算速度慢。为此，在其他传统ICP算法[7]的基础之上，提出基于 KDTree[8]的改进 ICP算法，包括基于KDTree搜索对应点对和矩阵变换参数的计算两方面的内容。

3.1 传统ICP配准算法

基本思路：在对应点云中搜寻最邻近点对，利用此最邻近点对求解刚体变换参数R、T，在这个过程中点对的搜寻和变换参数的求解都是迭代计算的。

算法步骤如下：

（1）令Ω为点云M和N的重叠域，设在Ω然数集N及其扩展情况，如正整数集Z+、n维实坐标中的任一点对应在M和N上的位置分别是Mi、Ni，初始迭代时两个点集的初始变换参数是 R0，T0。

（2）点集M中的每个点Mi，由初始变换参数最小为标准，求出新的变换参数 R、T。

（3）根据找到的全部最近点对（mi，ni），求出两个点集的变换参数R、T，并且以全部点对距离的平方和最小为标准，求出新的变换参数R、T。

（4）在相邻两次计算所得的距离平方和的差值小于给定的阈值时结束迭代，否则重复步骤（2）和（3）直至小于给定的阈值。

（5）根据最终得到的R、T将点云 M映射变换到点云N的坐标系下，完成配准。

3.2 改进的基于KDTree的ICP算法

3.2.1 算法准备工作

由KDTree的算法原理可知，当邻域点集中点数k为1时，搜寻点与邻域点间建立一一映射关系。此时，搜索到的邻域点是搜寻点与邻域点集中距离最小的点。

该算法中要用到的变换矩阵利用四元素法[9]求解，过程如下：

（1）求解点集 M、N 的重心坐标 O1、O2。

（2）点集 M、N的重心化：

（3）构建矩阵 Q：

（4）求解Q的最大特征值以及最大特征值对应的特征向量（w，m，n，p）。

（5）构造旋转矩阵：

（6）解算平移向量 T：

3.2.2 算法实现步骤

（1）设点集 M、N的部分区域分别为目标点集 M′和参考点集 N′。

（2）令 k=1，在 N′中通过 KDTree加速搜索为 M′中的任意点搜索最近邻域点，由此找出M′中任意一点的映射点，也就是找出 M′中点集合 Mm={，…，}在 N′上的映射点集 Nm={，…，}，m 代表迭代次数，n代表点个数。

（3）利用设置好的最小阈值距离 Di，删除 Mm、Nm中错误的点对，并完成Mm、Nm的更新。

（4）利用四元素法计算 Mm、Nm的变换矩阵 R和平移量 T。

（5）由得到的 R、T变换 Mm，得到最新的 Mm。

（6）重复步骤（2）～（5），求出 Mm中每一点到 Nm中的映射点对，以及相应的R、T。

（7）当最后的R、T满足配准后，对应点对坐标间差值的阈值收敛条件|xm-xn|or|ym-yn|or|zm-zn|<ε时，结束循环，匹配成功；如果不满足收敛条件，进行第 m+1次迭代计算。

算法设计流程如图4所示。

图4 基于KDTree改进的ICP算法实现

3.2.3 主要函数代码介绍

最小阈值Di设定函数：

坐标差阈值设定函数：

4 实验结果与结论

根据以上提出的算法，利用斯坦福大学实验室在不同坐标系下获得的兔子点云数据和实测的只有4个面数据的长方体的点云数据进行实验。

实验平台为Windows 8.1 64位操作系统，VS2010 32位，PCL点云库1.7.1。

如图5、图6所示，左上角和右上角为两个不同坐标系下的点云数据；图5左下角的右上方为利用传统ICP算法获得的实验结果，右下角的右上方为基于KDTree改进的ICP算法的实验结果；图6左下角的上方图为利用传统ICP算法获得的实验结果，右下角的上方图为基于KDTree改进的ICP算法的实验结果。

图5 兔子模型点云的配准效果

图6 长方体点云的配准效果

由以上比对可以明显看出，传统ICP算法获得的结果有着明显的匹配不到的地方，而利用改进的ICP算法获得的精细化匹配结果趋于完美，能够实现两坐标系下点云数据的精细化匹配。

[1]王蕊，李俊山，刘玲霞，等.基于几何特征的点云配准算法[J].华东理工大学学报（自然科学版），2009，35（5）：768-773.

[2]郑德华，岳东杰，岳建平.基于几何特征约束的建筑物点云配准算法[J].测绘学报，2008，37（4）：464-468.

[3]张政.点云数据配准算法研究[D].济南：山东大学，2008.

[4]YANG R， ALLEN P K.Registering， integrating， and building CAD models from range data[C].1998 IEEE International Conference on Robotics and Automation IEEE，1998，4：3115-3120.

[5]路银北，张蕾，普杰信，等.基于曲率的点云数据配准算法[J].计算机应用，2008，27（11）：2766-2769.

[6]BESL P J，MCKAY N D.Method for registration of 3-D shapes[C].Robotics-DL Tentative，International Society for Optics and Photonics， 1992： 586-606.

[7]ZINBER T， SCHMIDT J， NIEMANN H.A refined ICP algorithm for robust 3-D correspondence estimation[C].2003 International Conference on Image Processing， ICIP 2003，IEEE， 2003，3（2）：695-698.

[8]Zhang Zhengyou.Iterative point matching for registration of free-form curves and surfaces[J].International Journal of Computer Vision，1994，13（2）：119-152.

[9]HORN B K P， HILDEN H M， NEGAHDARIPOUR S.Closed-form solution of absolute orientation using orthonormal matrices[J].Journal of the Optical Society of America A， 1988， 5（7）： 1127-1135.