基于改进ICP算法的三维点云刚体配准方法

汪 霖，郭佳琛，张 璞，万 腾，刘 成，杜少毅

(1.西北大学 信息科学与技术学院,陕西 西安 710127;2.西安交通大学 人工智能学院,陕西 西安 710149)

对于不同光照、角度等条件下获取的两个三维点云,其刚体配准的目的是建立两个点云间的空间对应关系，并寻找它们之间的最优刚体变换关系[1],从而对齐空间中的两个点云。三维点云刚体配准广泛应用于智能机器人、无人车、医学和文物保护等领域[2-5]。

为了求解上述优化问题,Besl和Mckay于1992年提出了迭代最近点(iterative closest point,ICP)算法[6],其在解决噪声较小的刚体点云配准问题中具有一定的精度和速度优势。由于ICP算法要求点云是完全对应的,因此该算法对于含有噪声和外点的点云数据进行配准时,其配准精度较低。针对ICP算法在点云配准方面的不足,一些学者提出了其改进方法。Bae等人通过点云曲率的计算,构成空间内各点与其邻域点的协方差式,以提高匹配的收敛速度[7]；Zhang等人提出将噪声点进行删除处理,以提高刚体配准的鲁棒性[8]；Yang等人在分支定界的基础上,增加了点云运动特征信息,一定程度上改进了配准效率[9]；Luo等人通过距离视差的计算提高了ICP算法的配准精度[10]；Guan等人采用体素点阵对点云数据进行重采样[11],并使用kd-tree对点云对应关系进行优化[12],以提高ICP算法的配准精度。随着深度相机的发展,RGB-D点云的配准也逐渐受到学者的关注。MEN等人利用点云位置坐标结合色调信息,提出了四维ICP算法,但是该方法对噪声和外点的鲁棒性较差[13]；Korn等人利用Lab颜色信息对ICP算法进行改进,利用kd-tree搜索办法提高点云的匹配度,但该方法对采集环境的光照有一定要求,对于光照变化较大的场景其点云配准精度较低[14]；Danelljan等人结合颜色信息提出一种概率模型约束方法,但该方法不适合球面结构的点云配准[15]。

针对ICP算法对于存在噪声和外点的三维点云刚体配准的配准精度较低、鲁棒性差等问题,本文提出一种基于改进ICP的三维点云刚体配准方法。为了提高三维点云刚体配准方法的鲁棒性,建立了基于伪Huber损失函数的三维点云刚体配准模型。在此基础上,引入RGB-D点云颜色信息,辅助点云对应关系的准确建立,从而减少噪声和外点对配准结果的影响。由于Levenberg-Marquardt(LM)优化三维点云刚体配准模型时对初值较敏感,若初值选取不合适,将极大地影响点云配准精度,为此,将奇异值分解(singular value decomposition,SVD)和LM算法相结合,利用SVD算法求解的刚体变换关系作为LM算法的初值,从而提高点云配准精度。实验结果表明,本文所提三维点云刚体配准方法能够有效抑制噪声和外点对配准精度的影响,其三维点云配准精度高。

1 ICP算法简介

ICP算法,由于其迭代方式简单、快速,被广泛应用于三维点云刚体配准中。ICP算法的目标是寻找一个最优刚体变换,使得形状点云S和模型点云M能够在空间上对应起来。

假设形状点云和模型点云的刚体变换关系中,旋转矩阵为R,平移向量为t,则三维点云刚体准优化问题可写为

s.t.RTR=I3,det(R)=1。

(1)

其中,c(i)为i的对应点。

ICP算法采用迭代求解旋转矩阵R和平移向量t。在每一次的迭代中,都有以下3个步骤:

步骤1基于第k-1次的刚体变换Rk-1和tk-1,建立点云之间的点对应点关系:

(2)

(R*,t*)=

(3)

更新第k步的刚体变换Rk和tk:

Rk=R*Rk-1,tk=R*tk-1+t*。

(4)

步骤1中,ICP算法采用点对点的搜索策略建立两个点云之间的对应关系,通常采用基于Delaunay三角化[16]的最近点搜索方法,以提高搜索效率。该方法首先对模型点云进行Delaunay三角剖分,再采用三角划分的搜索策略找到两个点云的对应点。

而对于步骤2中刚体变换求解问题,ICP算法通常采用SVD方法对其进行求解。首先给出以下两个定理。

s.t.RTR=Im,det(R)=1。

(5)

能够得到R*=VDUT,其中U为m×m维正交矩阵,V为m×m维正交矩阵,D矩阵的定义为

(6)

求得最优的旋转矩阵R*后,最优平移向量t*为

(7)

最后,根据式(4)更新第k步的刚体变换Rk和tk。

2 基于改进ICP算法的三维点云刚体配准方法

2.1 算法描述

在ICP算法的基础上,本文提出了基于改进ICP的三维点云刚体配准方法。为了提高配准方法对噪声和外点的鲁棒性,将对噪声和外点不敏感的伪Huber损失函数引入到三维点云刚体配准优化模型中。考虑到基于Delaunay三角化[16]的最近点搜索方法建立点云之间的对应点关系会受到点云中噪声和外点的影响,可能形成错误的对应关系,从而降低配准精度。为此,本文方法引入颜色信息作为约束条件之一,结合RGB-D颜色信息和位置坐标信息,建立点云之间更为精准的对应关系。最后通过迭代求解的方式,当误差达到设定阈值或迭代次数达到最大时,得到最优的三维点云刚体变换关系。

2.2 基于伪Huber损失函数的点云刚体配准优化模型建立

为了提高三维点云刚体配准方法在复杂噪声环境下的鲁棒性,需要有效抑制噪声和外点对三维点云刚体配准优化模型的影响。为此,本文通过引入对噪声和外点不敏感的伪Huber损失函数，建立两个点云刚体配准的数学模型。

伪Huber损失函数是Huber损失函数的平滑版,具有连续可导性,其函数定义为

(8)

其中：b为噪声和外点阈值；a为误差。对于较小的a值,该损失函数近似值为a/2，而对于较大的a值,该损失函数可近似为一条斜率为b的直线,因此，其对噪声和外点不敏感。

将ICP算法的优化函数(1)中的度量函数替换为伪Huber损失函数,可得到如下的基于伪Huber损失函数的三维点云刚体配准优化模型:

s.t.RTR=I3,,det(R)=1。

(9)

2.3 基于改进ICP的三维点云刚体配准方法

2.3.1 总体流程图 结合ICP算法的思想和本文方法的优化目标函数,可以给出本文方法的总体流程图，如图1所示。本文方法的步骤与ICP算法相似,重点在于迭代过程中点云间对应关系的建立和求解,以及根据优化目标函数(9)计算新的刚体变换两个步骤。

图1 本文方法的总体流程图Fig.1 Overall flowchart of the method

2.3.2 对应关系的建立和求解 根据点云的位置分布特征和颜色信息的引导,能够得到更加准确的点云对应关系。本文采用HSV颜色空间模型,其中H、S和V分别表示色相(hue)、饱和度(saturation)和色调(value)。由于色相值能够更好地反映颜色的本质,并且其对光照度的变化不敏感,故本文只利用色相值进行颜色辅助。通过引入色相值,建立点云之间的点对应点关系,将式(2)改写为

(10)

对于对应点关系的求解,本文同样采用Delaunay三角化方法。通过引入颜色辅助信息,能够获得比ICP算法更为精确的点云间对应关系。两组点云之间的对应关系建立效果对比如图2所示。

图2 两组点云对应关系建立效果对比Fig.2 Comparison of the establishment effect of the two sets of point cloud correspondence

(11)

由于LM优化算法对于初值选取较为敏感,本文采用SVD方法和LM算法相结合的优化方法对模型(11)进行求解,即利用传统ICP算法中SVD方法求解优化模型(11)的初值,再利用LM算法对优化模型(11)进行求解,以精确求解R*和t*。在此基础上,更新第k步的刚体变换Rk和tk:

Rk=R*Rk-1,tk=R*tk-1+t*。

(12)

3 实验结果及分析

为了便于描述,将本文所提的基于改进ICP算法的三维点云刚体配准方法记为RPH-ICP方法。本节将对RPH-ICP方法和ICP算法进行三维点云刚体配准实验,以验证本文算法的有效性。

3.1 实验环境设置

实验中,采用标准数据库和仿真数据对RPH-ICP方法和ICP算法进行点云刚体配准实验对比。标准数据选取于A Large-Scale Hierarchical Multi-View RGB-D Object Dataset[17]中的earth与foodbag数据。考虑数据采集中噪声的影响,标准数据中本身带有噪声。此外,将模型点云沿z轴从0°到60°的随机角进行旋转,得到其目标点云。再加入部分随机噪声和均值为8、方差为2的高斯噪声得到待配准形状点云。采用模型点云与形状点云的均方根误差(root mean square,RMS)、迭代次数,以及旋转矩阵估计误差εR=‖R估计-R真实‖2和平移向量估计误差εt=‖t估计-t真实‖2作为点云配准的性能指标,对RPH-ICP方法和ICP算法的点云配准效果进行比较。

3.2 三维点云刚体配准的实验结果及分析

实验分别采用本文方法(RPH-ICP方法)和ICP算法进行三维点云的刚体配准。通过实验结果对比,以验证本文方法的收敛性与刚体配准精度。两种方法的配准结果数据如表1所示。由表1可知，本文方法得到的RMS误差值比ICP算法更小,并且旋转矩阵R和平移向量t的估计误差εR和εt均小于ICP算法,本文所提方法的点云配准精度高,其原因是本文方法所引入的伪Huber函数能够有效抑制噪声和外点的影响。

表1 两种三维点云刚体配准方法的实验结果Tab.1 Experimental results of two rigid registration methods of 3D point cloud

图3给出earth数据的收敛图以及foodbag数据在不同外点比例下的收敛图,由图3可知,本文方法的收敛速度比ICP算法快,并且本文方法对于外点具有很好的鲁棒性。

为了更直观地显示两种方法的配准结果,图4和图5给出了两种方法对于earth数据和foodbag数据的三维点云刚体配准结果。由图4和图5可知,本文方法配准后在各视角下蓝色点与红色点的重合度均高于ICP算法,因此,本文方法能够有效抑制噪声和外点的影响，配准精度高。

图3 点云配准收敛曲线Fig.3 Registration convergence curve of point cloud

图4 本文方法和ICP算法配准结果比较(earth数据)Fig.4 The comparison of registration results between the proposed method and ICP algorithm (earth data)

图5 本文方法和ICP算法配准结果比较(foodbag数据)Fig.5 The comparison of registration results between the proposed method and ICP algorithm (foodbag data)

4 结语

为了有效抑制噪声和外点对于三维点云刚体配准精度的影响,提高点云配准方法的精度和鲁棒性，本文提出了一种基于改进ICP算法的三维点云刚体配准方法。该方法通过引入伪Huber损失函数建立鲁棒的三维点云刚体配准优化模型,利用HSV颜色空间的色相值辅助建立更为准确的点云对应关系,从而提高算法对于噪声和外点的抗干扰能力。实验结果表明,本文所提出方法能够有效抑制噪声和外点的影响，点云刚体配准精度高。