改进的迭代最近点快速点云拼接算法

崔晓云, 张二锋

(1.西安邮电大学 数字艺术学院，陕西 西安 710121； 2.清华大学 教育研究院，北京 100084)

三维点云技术[1]是三维场景重建的关键技术之一，其实现原理主要是通过生成点云形态即通过对场景中目标物体的点匹配重建，从而完成三维点云重建。现已广泛应用在军事、工业和医疗等多个邻域。

迭代最近点( iterative closest point，ICP) 算法[2]是一种点云匹配算法，通过旋转矩阵及平移向量，根据有效点映射关系对不同坐标系下的数据点云进行坐标变换。该算法能够精准地完成三维点云配准，但是对于迭代输入值要求极高，其源点云与目标点云位置接近且两片点云之间存在包含关系。若这些条件没有达到，则会影响配准精度及收敛速率。因此，国内外众多学者致力于ICP算法的研究及改进[3-4]。点云初始配准的优化求解算法[5]基于序列图像运动规律，能以较小的计算量获取近似全局最优的点云初始配准结果，从而实现点云配准；通过添加带边界的尺度矩阵，引入动态因子，保持了算法的尺度不变性[6]；将正态分布变换算法与ICP算法相结合，先通过正态分布变换算法进行初始配准，然后采用两次ICP进行微调和细配准，提高了算法的配准精度[7]。但是，上述算法仅提高了点云拼接技术的配准精度，未涉及点云彩色信息的匹配。

为了更好的提高配准精度及收敛精度，本文提出一种改进的迭代最近点云快速拼接算法。利用NCAM光学[8]追踪彩色与深度信息，获取原始点云数据点；将色彩特征点与随机采样相结合，进行特征点采样，获取有效的初始点云数据；引入伞曲率体积积分不变量特征点描述算法[9]，避免因复杂光照环境影响而生成的大量不可靠信息点，实现误匹配点剔除；同时构建带色彩约束的目标函数，并通过四元数法[10]和特征值分解法[11-12]求解最优值，实现坐标变换求解。

1 NCAM光学追踪系统架构

以光学追踪定位系统为基础，利用图形图像识别[13]和光学技术，通过多传感器摄像头和定制服务器，大面积地获取具备色彩和深度数据的三维坐标空间的点云信息。系统包括数据采集、数据处理及视频合成输出渲染。NCAM光学追踪系统架构[8]如图1所示。

图1 NCAM光学追踪系统架构

将摄像机里具有“跟踪功能”的深度摄像机NcamLive配合传感器Camera Bar双捕捉摄像头，实时光学追踪并获取待采集的信息数据，从而确定此目标图像的确切位置以及转动信息。该摄像头实时追踪获取环境信息、摄像机镜头参数Zoom及Focus等数据，统一通过数据接口盒，以Cat6网线的级联方式输送至虚拟跟踪服务器，在服务器完成整个点云数据信息点的拼接，最后将虚拟跟踪服务器计算得出的跟踪数据按照Free D协议编码，输出至ORAD渲染服务器完成视频合成显示。

2 改进的ICP点云拼接算法

ICP算法主要包括原始点云数据获取、初始对应点云集的确定、误匹配点剔除和坐标变换求解等4个部分。具体实现步骤如下。

步骤1 原始点云数据获取。在保留物体表面关键信息点的基础上，减少噪声点，完成数据采样。

步骤2 初始对应点云集的确定。在源点云集P中选取第i个点的空间三维向量Pi，i=1,2,…,np，np为提取的源点云集P的点数。在目标点云集Q中找到Pi最近邻点Qj，j=1,2,…,nq，nq为提取的目标点云集Q的点数。

步骤3 误匹配点剔除。引入评价标准、约束条件去除不可靠数据点，完成误匹配点剔除。

步骤4 坐标变换求解。利用最小二乘法迭代求解，获取坐标变换。

在源点云数据与待配准的点云数据之间寻找距离最近点是整个ICP算法的关键环节。利用最小二乘法，在点云重叠区域获取最近点的时间复杂度O(nplognq)高，因此，按照配准点色调h相似性原理，对ICP算法进行改进。

改进的ICP点云拼接算法结构如图2示。

图2 改进的ICP点云拼接算法结构

利用NCAM光学追踪彩色与深度信息，并建立彩色点云模型；将色彩特征点与随机采样方法相结合，进行特征点采样对点云数据进行划分；引入伞曲率体积积分不变量特征点描述算法，剔除误配准点，利用k-d树邻域搜索法[14]建立空间索引结构，缩小最近点搜索范围，以此构建约束函数，依据均方根误差[15]对配准误差进行评价，保证对应点对的查找准确性，实现点云配准。

2.1 数据采集

将深度摄像机的深度距离范围限制在1 300～3 500 mm之间，此范围内的像素值设为1，除此之外的均设置为0。Camera Bar捕捉摄像头的镜头曝光设置为8 500～9 000，gamma设置为0.5左右，以60帧/s频的采样率，利用NCAM光学追踪系统实时采集待拼接的三维点云。

2.2 彩色点云模型构建

通过NCAM获取的彩色点云信息，不但包含空间位姿信息，还包含三维点云信息红、黄、蓝三通道的r、g、b像素值，取值范围均为[0,255]。考虑到目标信息点受实际复杂环境灯光的影响，r、g、b值误差较大，而色调h受灯光影响较小，特用每个点的归一化色调值[16]代表色彩信息，以此构建彩色点云模型，从而实现对输入值的优化处理。色调h的计算表达式为

(1)

其中R、G、B分别为r、g、b值除以像素总和得到的归一化色彩分量值，取值范围均为[0，1]。点云信息RGB值归一化如图3所示。

图3 点云信息RGB值归一化

在实现源点云集P和目标点云集Q的拼接过程中，对不同视角下的两片点云RGB值根据式(1)进行归一化处理，得到源点云集合

(2)

目标点云集合

(3)

2.3 特征点采样

在覆盖整个彩色点云模型的基础上，为了避免因局部色调比重较大，导致随机采样过程中大部分的采样点相同或者色调较为相近的问题，利用色彩特征点与随机采样相结合的方法实现彩色点云特征点采样，保证采样空间及色彩的均匀分布同时能够覆盖整个模型。具体操作步骤如下。

步骤1 将色调h值从0至1均分为n份，将点云集P划分为n个子点云集Pk(k=1,2,…,n)，则Pk仅包含满足色调h的点，此时h取值范围为{(k-1)/n}≤h<(k/n)。

步骤2 将点云集Q划分为n个子点云集Qw(w=1,2,…,n)，则子点云集Qw仅包含满足色调h的点，此时h取值范围为{(w-1.5)/n}≤h≤{(w+0.5)/n}。

以n=4为例，子点云划分如图4所示。

图4 子点云划分

步骤3 特征点采样。选择Pk中的任一点作为特征点，通过迭代方式，获取所有有效的特征点并储存至服务器。

由图4可知，源点云集Pk中所有的点仅需要在Qk中寻找，而Qk覆盖的色调范围大于Pk，因此，处于色调边缘的所有Pk点均能够找到与之相匹配的点，从而减少误差。同时为保证采样点空间分布均匀且色彩信息丰富，故而将n值设置为15，能够有效避免色彩特征点局部拟合问题[17]。

2.4 特征描述

引入伞曲率体积不变量特征点描述算法[18]，利用k-d树邻域搜索法确定待搜索点P的8个邻域点，以此特征点求解特征值，完成特征点描述。源点云集P的特征值表达式为

(4)

其中wk为第k个邻域点的权重，UPk为P的第k个邻域点的特征值。通过各个邻域点做出的贡献设置权重，同时给定阈值，当权重在阈值范围内，则可作为有效特征点，否则剔除该点。同理，可计算目标点云集Q的特征值，完成特征点描述。

除此之外，还将对因摄像机硬件及抖动而产生的噪声点和离群点，采用改进的Multi-Quadri滤波算法[18]去除点云数据中分布不规则，且在一些区域发生随机抖动的无效数据点来进行噪声处理。

2.5 目标函数构建

点云集P和Q之间满足三维刚体变换关系，在第m次迭代过程中，刚体变化的旋转矩阵记为R(m)，平移矩阵记为T(m)，以此构建带色彩约束的目标函数

(5)

根据均方根公式[19]

(6)

假设色调分布均匀的情况下，目标函数的计算复杂度为O(nplog(nq/n))，而实际计算过程中，色调不是均匀分布的，除主色调之外的其它色调占比较少，与之对应的特征点也较少，故而实际计算过程中算法的复杂度会更低。

3 改进的ICP算法实现

改进的ICP算法的实现分为初始配准和精确配准两部分，具体步骤如下。

初始配准

步骤1 通过NCAM光学追踪获取彩色与深度点云信息。

步骤2 对获取的点云信息，构建彩色点云模型。

步骤3 依据式(1)对r、g、b像素值进行归一化处理，得到式(2)和式(3)，即源点云集合P和目标点云集合Q。

步骤4 根据旋转平移位姿关系，完成初始配准。

精确配准

步骤5 对P和Q利用色彩特征点与随机采样相结合的方法获取有效子点云集Pk和Qw。

步骤6 为点云集Pk和Qw建立k-d数据结构并进行邻近搜索，根据式(4)计算点云集Pk和Qw的特征值，完成特征点描述。

步骤7 在Qw中寻找与源点子云集Pk色调一致的子点云集Qk。

步骤8 根据式子(5)对点云集P和Q构建带色彩约束的目标函数E(m)，并利用四元数法和特征值分解法获取R(m)和T(m)的最优值。

改进的ICP算法拼接流程如图5。

图5 改进的ICP算法拼接流程

4 实验设计及结果分析

为了验证本文算法的时效性及精确性，搭建此次实验场景。实验所需的服务器配置操作系统为Windows 10，主频2.60 GHz，CPU为Intel Core i7，内存为8GB；程序开发引擎Unreal Engine 4.20.3，开发软件为Visual Studio 2008和Matlab，开源库包含PCL点云库和OpenGL等；摄像机选取索尼FS4(含镜头)。

4.1 改进的ICP算法拼接效果

利用Matlab将彩色图像生成两组点云数据，通过改进的ICP算法实现拼接。待拼接的P与Q点云数据模型及初始位姿如图6所示。

图6 两组数据及初始位姿

从图6可以看出，两组数据的初始位姿无明显对应关系，此时，无法直接利用ICP算法进行精确配准。为了确保精准匹配，依据图6所表示的旋转平移映射关系，对两组数据进行初始配准，如图7所示。

图7 初始配准后的位姿

依据改进的ICP算法，对每个点的归一化色调值处理，建立点云k维数据空间的数据结构，搜过最近邻点，构建约束函数，从而求解最优值，获取得到精确配准的输入初始位姿如图8所示。

图8 初始位姿

在精确配准的过程中，将迭代次数设置为80 s，同时考虑到不同的追踪目标其构建的函数不同，故在每运行10 s时，记录下当前迭代次数及函数的最优解。配准结果如图9所示。

图9 配准后的位姿

从图9可以看出，改进算法的拼接效果较好，鼻子、身体、足部轮廓显著，但是尾部轮廓效果有所偏差。这是因为鼻子、身体、足部特征点分布集中，无明显差异；而尾部特征点分布较少，因而差异较大。

在精确配准的过程中，仍然会有大部分噪声会显现出来，但是，改进的ICP算法加入了色彩约束条件可有效的剔除了大部分噪声，并且提高收敛速率及拼接精度。

4.2 拼接精度及收敛速率对比分析

依据均方根误差分别对比ICP算法和改进的ICP算法的配准误差，利用迭代时间对配准收敛速率进行评价，实验结果分别如图10和图11所示。

图10 迭代精度曲线

图11 迭代时间曲线

从图10和图11分别可见，ICP算法在38次的迭代，耗时28.98 s的情况下，RMS值为0.039 1；改进算法在18次迭代、耗时5.78 s的情况下，RMS可达到0.033 9，表明改进算法受周围环境及人为因素影响较小，程序可自动进行计算，故而在初始配准之后，仍然能够达到较高的配准精度；在完成初始配准之后将结果作为输入值进行细配准，加入约束条件快速的去除色调不一致的点对，能够进一步加快其迭代收敛速率，提高运算速率及匹配精度。

5 结语

改进的ICP点云快速拼接算法，基于NCAM光学追踪彩色与深度信息，根据彩色与深度点云信息匹配点色调相近性，利用k-d树邻域搜索法加速对应点对的匹配，以点到切平面距离作为迭代的目标，建立目标约束函数，最后利用特征值分解法最优化算法对目标函数进行求解。实验结果表明，与ICP算法相比，改进算法的拼接匹配率高，同时迭代精度明显提升，迭代时间大幅缩短，可较好地提高拼接质量。