CN107123138A - 基于vanilla‑R点对剔除策略的点云配准算法 - Google Patents

Abstract

本发明公开了一种基于vanilla‑R点对剔除策略的点云配准算法，它对现有点到点的点云配准算法中关联点对的剔除策略进行了改进，能有效增加点云配准算法的鲁棒性，获取更为准确的点云配准。本发明所设计的vanilla‑R点对剔除策略具体为通过随着迭代逐渐下降的模型最近点搜索距离阈值和互邻距离阈值来共同作用剔除噪声点对。通过两个策略的共同作用，可以保证点云准确、高效地配准。此外，本发明对随机采样点数的设置鲁棒性高，甚至少量的选点就能实现准确的配准，从而减少了运行时间，这是以往其他算法难以实现的。最后，本发明对算法收敛条件进行了设计，保证了该算法能够有效的收敛到一个全局最优解。

Description

基于vanilla-R点对剔除策略的点云配准算法

技术领域

本发明属于计算机图形学技术领域，具体涉及一种基于vanilla-R点对剔除策略的点云配准算法的设计。

背景技术

随着计算机技术和传感技术的不断发展，计算机图形学作为计算机科学中一个重大的分支，在当今社会倍受关注。如近年来火热的虚拟现实和增强现实等技术，都对图形处理提出了新的挑战。点云配准作为计算机图形学中一个十分重要的技术，被广泛用于图形处理的进程中。例如数字城市三维建模，通常利用三维激光扫描仪扫描周围的环境，然后再利用点云配准，将这些点云拼接在一起，从而可以在计算机中重建城市的三维原貌。在智能移动机器人的地图构建中，点云配准算法也是一个十分重要的环节。

过去的三十年里，涌现了各种各样的点云配准算法，经典的点云配准算法为Besl和McKay在1992年提出的最近点迭代算法(Iterative Closest Point，ICP)。经典的ICP算法，假设了两个待配准的点云数目相同而且没有噪声点的存在，并需要一个合理的初始值。但是这样的条件在实际的应用中是很难满足的。在随后的发展中，对点云配准算法的众多研究主要集中在如何找到正确的点对关系，这对实现正确的配准是极其重要的一个环节。一个最简单的方法就是通过设置一个点对距离阈值，当模型点对的距离超过这个阈值的时候，则剔除这个点对，由于在迭代的过程中模型点对的距离应逐渐趋近于零，因此，vanillaICP方法在迭代的过程中通过减少模型点对的距离阈值来剔除外部点。然而，这样的方法在很多情况下仍然无法获得准确的配准效果。除vanilla ICP方法外的研究工作主要有以下几种方法：

(1)1995年提出了ICRP(Iterative Closest Reciprocal Point)算法，先在点云M中利用最近点原则去关联点云G中的点，记为(g_k,m_k)，再在点云G中去找m_k最近的点g′_k。如果g_k与g′_k距离超过一个阈值，则拒绝这个配对关系。

(2)2002年提出的Trimmed ICP算法，首先估计出两次点云的重合度，如常用存在n个点对属于公共源来描述这个重合度，然后把根据最近点原则配对的点对的距离按照升序进行排序，选前n个点对作为正确的配对关系用于求解两次点云之间的旋转和平移关系。

(3)2003年提出了picky ICP，关键思想是为了防止同一个点出现在了多个点对的当中，若出现这种情况，则距离最近的那个点对被接受，其他的点对则被舍弃。

(4)2009年提出了frustum ICP算法，frustum ICP则起源于计算机图形学中的投影剔除，它通过创建一个视觉椎体来表示两次扫描可能的重合区域，如果点对落在了这个重合区域内则保留，否则剔除。

以上方法都在一定程度上剔除了局外点，但依然存在很多局限，主要表现在：

(1)没有任何一个方法从理论上可以证明能够完全剔除错误点对。如配对点的距离作为外部点对剔除的方法，实际是对误差进行一个合理的估计，这样虽然可以较好的剔除不属于公共源部分的点对，但是没有考虑公共源中点对错误配对的情况。ICRP则认为点对之间的关系应该是对称的，因此使用这种思想来剔除错误配对，但是外部的点对仍然有可能满足该关系。picky ICP认为点对的配对关系应该是唯一的，但是当某个点出现在多个点对出现的时候，选择的点对剔除方法可能使得正确的配对关系被剔除，从而错误的配对关系被保留。尤其是当一个平面绕它的法向量方向旋转一定角度，而且两次配对点云中存在着大量的非公共源的点时，使用picky ICP方法无法收敛到正确的值。

(2)各种方法之间存在着一定的互补和冗余。实际上，ICRP和picky ICP在外部点点对剔除中有时会产生相同的效果，因为点对关系对称与点对关系唯一在一定程度上是等价的，相比picky ICP因减少了一个最近邻搜索，从而在配准的速度上更快，但是在鲁棒性方面不如ICRP。甚至picky ICP不满足Besl和McKay在ICP算法中提出的误差收敛条件，因此可能导致迭代被无限次的进行下去。Trimmed ICP选用前n个点对视为正确的配对关系，但是如果vanilla ICP选择的距离阈值与第n个点对的距离阈值相同时，这两个方法将会点对剔除上取得相同的效果。frustum ICP尽管比vanilla ICP对噪声点更加鲁棒，但是由于需要创建视觉椎体，这种方法对于全方位的三维激光扫描仪来说并不适用。由于以上方法的局限性，在实际的应用如机器人大规模的三维建图中，使用以上的ICP算法进行点云配准时会存在较大的误差，并且随着误差的积累，难以保证建图的一致性。

对于点到平面的ICP方法，需要求出已知点云的平面模型，这个问题在实际应用中则需要大量时间进行3D霍夫变换以求得已知点云的平面法向量。本发明专利，旨在提出一种改进的点到点的点云配准算法，其关键在于即使存在着大量的非公共源点对时也能剔除外部点对，使之能收敛到一个正确的解。

发明内容

本发明的目的是为了解决现有的ICP算法进行点云配准时会存在较大的误差，并且随着误差的积累，难以保证建图的一致性的问题，提出了一种基于vanilla-R点对剔除策略的点云配准算法。

本发明的技术方案为：基于vanilla-R点对剔除策略的点云配准算法(vanilla-RICP)，包括以下步骤：

S1、初始化模型参数R₀和t₀，R₀表示待配准点云到目标点云的初始旋转矩阵，t₀表示待配准点云到目标点云的初始平移向量；设置随机选点数N、初始距离阈值d_M、d_M的下降步长S_d、互邻距离阈值d_R以及收敛参数α、β和n_cvg；

S2、将目标点云A按照k-d tree的数据结构进行存储；对待配准的点云B中所有点b_i应用当前模型参数R_k和t_k进行转化，即m_i＝R_kb_i+t_k，其中下标k为求解模型参数的迭代次数；m_i组点构成了点云M，将其按照k-d tree的数据结构进行存储；

S3、在M中随机选择点m_i，并使用最近邻搜索算法在集合A中找出去它的最近点a_j，形成点对(a_j,m_i)；

S4、判断点对(a_j,m_i)是否满足vanilla-R原则，若满足则将点m_i替换为点云B中对应的点b_i，即形成了模型点对(a_j,b_i)，并存储为(y_n,p_n)，进入步骤S5，否则返回步骤S3；

S5、判断当前存储的点对数目n是否达到随机选点数N，若是则进入步骤S6，否则返回步骤S3；

S6、使用奇异分解求解出已经存储的N对点对(y_n,p_n)的旋转和平移变换关系，得到新的模型参数R_k和t_k；

S7、根据公式得到误差error_k，判断其是否满足收敛条件，若收敛则算法结束，否则根据下降步长S_d减少距离阈值d_M，替换模型参数，返回步骤S2。

进一步地，步骤S4中vanilla-R原则具体为：

当前点对(a_j,m_i)的距离应小于当前距离阈值d_M，如果不满足则返回步骤S3；在满足前一个条件时，在点云M中去搜索a_j的最近点m′_i，并判断m_i与m′_i的距离是否小于互邻距离阈值d_R。

进一步地，步骤S6包括以下分步骤：

S61、计算出两个点云的几何中心，即：

S62、计算出矩阵

S63、对W矩阵进行奇异值分解，求出U和V矩阵；

S64、计算出旋转矩阵R_k＝USV^T，其中S是一个3阶的对角矩阵，对角元素为{1 1detU·detV}，det U和det V表示U和V矩阵的行列式；

S65、计算出平移向量t_k＝y_c-R_kp_c。

进一步地，步骤S7中的收敛条件为：

首先，要求误差error_k必须低于人为设置的阈值θ_er，θ_er＝αN；再判断误差error_k是否收敛，即是否满足|error_k-error_k-1|＜ε_er，ε_er＝βN；为了防止随机选点的影响，设置了一个收敛计数n_cvg，当以上条件出现n_cvg次数时，才会认为满足要求的收敛已经达到。

本发明的有益效果是：本发明对现有点到点的点云配准算法中关联点对的剔除进行了改进，提出了基于vanilla-R点对剔除策略的点云配准算法(vanilla-R ICP)，增加了点云配准算法的鲁棒性，有利于更为准确的点云配准。本发明所设计的vanilla-R原则具体为通过设置会随着迭代逐渐下降的最近点搜索距离阈值和互邻距离阈值来共同作用剔除噪声点对。前一个策略可以一定程度上有效的替代trimmed ICP和frustum ICP中的点对剔除策略，并且减少了运算时间。互邻距离阈值的设计有效的解决了picky ICP在点对拒绝中容易剔除正确点对的问题。通过两个策略的共同作用，可以实现高效、准确的点云配准，尤其是在两个点云集不存在大量的公共源部分时，效果尤为显著。并且使用该方法，对随机采样点数的设置鲁棒性高，甚至少量的选点就能实现准确的配准。收敛条件的设计，保证了本发明能够有效地收敛到一个全局最优解。

附图说明

图1为本发明提供的基于vanilla-R点对剔除策略的点云配准算法流程图。

图2为本发明实施例提供的两个待配准的点云图。

图3为本发明实施例的采用vanilla ICP算法得到的点云配准效果图

图4为本发明实施例的采用vanilla-R ICP算法得到的点云配准效果图。

具体实施方式

下面结合附图对本发明的实施例作进一步的说明。

本发明实施例采用一个开源的三维扫描数据集，其中包含了三维点云数据和里程计数据，数据下载链接为http://kos.informatik.uni-osnabrueck.de/3Dscans/，数据编号为5。本发明实施例选用了前两次扫描点云数据，在这里分别记为点云A和点云B，点云A和点云B部分重叠。点云A为机器人在初始位置使用三维激光雷达扫描得到的走廊三维点云数据，点云B为机器人在前进了大约两米并发生了小幅度的旋转后使用激光雷达扫描得到的走廊三维点云数据。这里将利用本发明提供的改进的点云配准算法对这两个点云数据集进行配准，以保证前后两次扫描的点云的公共源部分能够正确的重合，从而使得两次扫描数据得到正确的配准。值得注意的是，使用该算法仅需要找到满足条件的500个点就能实现准确的配准，这是其他任意一种方法都难以实现的。

如图1所示，本发明的具体步骤如下：

S1、初始化模型参数R₀和t₀，R₀表示待配准点云到目标点云的初始旋转矩阵，t₀表示待配准点云到目标点云的初始平移向量；设置随机选点数N、初始距离阈值d_M(与最远点到原点的距离成正比)、d_M的下降步长S_d(影响收敛速度和效果)、互邻距离阈值d_R以及收敛参数α、β和n_cvg，各参数的具体数值如表1所示。

表1

参数	dM	Sd	dR	N
					取值	1010	25	6	500

本发明实施例中，从下载数据名为scan001.pose的数据文件读取里程计数据，初始化模型参数R₀和t₀，R₀和t₀表示机器人从第二个位置到达第一个位置发生的旋转和位移转换关系。

S2、将目标点云A按照k-d tree的数据结构进行存储；对待配准的点云B中所有点b_i应用当前模型参数R_k和t_k进行转化，即m_i＝R_kb_i+t_k，其中下标k为求解模型参数的迭代次数；m_i组点构成了点云M，将其按照k-d tree的数据结构进行存储。

S3、在M中随机选择点m_i，并使用最近邻搜索算法在集合A中找出去它的最近点a_j，形成点对(a_j,m_i)。

S4、判断点对(a_j,m_i)是否满足vanilla-R原则，若满足则将点m_i替换为点云B中对应的点b_i，即形成了模型点对(a_j,b_i)，并存储为(y_n,p_n)，进入步骤S5，否则返回步骤S3。

该步骤具体为：

判断当前点对(a_j,m_i)的距离是否小于当前距离阈值d_M，如果不满足则返回步骤S3；否则继续在点云M中去搜索a_j的最近点m′_i，并判断m_i与m′_i的距离是否小于互邻距离阈值d_R，如果满足则将点m_i替换为点云B中对应的点b_i，即形成了模型点对(a_j,b_i)，并存储为(y_n,p_n)，进入步骤S5，否则返回步骤S3。

S5、判断当前存储的点对数目n是否达到随机选点数N，即n＝N是否成立，若成立则进入步骤S6，否则返回步骤S3。

S6、使用奇异分解求解出已经存储的N对点对(y_n,p_n)的旋转和平移变换关系，得到新的模型参数R_k和t_k。

该步骤具体包括以下分步骤：

S61、计算出两个点云的几何中心，即：

S62、计算出矩阵

S63、对W矩阵进行奇异值分解，求出U和V矩阵。

S64、计算出旋转矩阵R_k＝USV^T，其中S是一个3阶的对角矩阵，对角元素为{1 1detU·detV}，det U和det V表示U和V矩阵的行列式。

S65、计算出平移向量t_k＝y_c-R_kp_c。

S7、根据公式得到误差error_k，判断其是否满足收敛条件，若收敛则算法结束，将R_k和t_k作为最终求解结果；否则减少距离阈值d_M＝d_M-S_d，替换模型参数，返回步骤S2。

由于随机选点使得模型误差会出现小幅度的波动，因此在设计收敛条件时采用了一些小的技巧。首先要求误差error_k必须低于人为设置的阈值θ_er(它与N成正比，θ_er＝αN)，再判断是否收敛，即：|error_k-error_k-1|＜ε_er(它与N成正比，ε_er＝βN)。为了防止随机的影响，设置了一个收敛计数n_cvg，当以上条件出现n_cvg次数时，才会认为满足要求的收敛已经达到。其具体的参数如表2所示。

表2

参数	α	β	ncvg
				取值	2.1	0.2	3

对点云B中点云应用最终求得的旋转及平移矩阵R_k和t_k，并统一到点云A中。通过可视化即可得到图4所示的点云配准效果。

表3给出了针对开源三维扫描数据集(其中包含了三维点云数据和里程计数据，数据下载链接为http://kos.informatik.uni-osnabrueck.de/3Dscans/，数据编号为5)，vanilla-R ICP算法和其它算法的运行时间和误差数据对比。

表3

算法名称	vanilla ICP	trimmed ICP	picky ICP	vanilla-R ICP
					运行时间(s)	1.9035	6.02731	2.88134	1.86003
误差	37627.6	30697.0	39966.9	13126.1

由于当点云配准后，所有属于公共源的点对误差理论上应为零，因此，把所有点对配准误差按照升序排列，选出点对误差排在前n(n是用来描述两次点云重复度的，本发明实施例中n＝5000)的点对，将其配准误差之和作为误差数据用以分析。可以发现，无论是准确度还是运行时间，本发明所提出的vanilla-R ICP算法都优于其他算法。图2为本发明实施例所提供的两次待配准的点云图。图3为使用vanilla ICP对图2中点云进行配准得到的效果图，从图中圈出的部分可以发现，前后两次扫描中原本属于同一个墙壁的点云没有得到准确的配准，意味着整体配准仍存在着较大的误差。图4为使用本发明vanilla-R ICP所得到的点云配准效果图，对比图3可以发现，本发明所提出的算法明显改善了vanilla ICP的配准效果，其结果也在误差数据上得到了证实。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (4)

1.基于vanilla-R点对剔除策略的点云配准算法，其特征在于，包括以下步骤：

S1、初始化模型参数R₀和t₀，R₀表示待配准点云到目标点云的初始旋转矩阵，t₀表示待配准点云到目标点云的初始平移向量；设置随机选点数N、初始距离阈值d_M、d_M的下降步长S_d、互邻距离阈值d_R以及收敛参数α、β和n_cvg；

S2、将目标点云A按照k-d tree的数据结构进行存储；对待配准的点云B中所有点b_i应用当前模型参数R_k和t_k进行转化，即m_i＝R_kb_i+t_k，其中下标k为求解模型参数的迭代次数；m_i组点构成了点云M，将其按照k-d tree的数据结构进行存储；

S3、在M中随机选择点m_i，并使用最近邻搜索算法在集合A中找出去它的最近点a_j，形成点对(a_j,m_i)；

S4、判断点对(a_j,m_i)是否满足vanilla-R原则，若满足则将点m_i替换为点云B中对应的点b_i，即形成了模型点对(a_j,b_i)，并存储为(y_n,p_n)，进入步骤S5，否则返回步骤S3；

S5、判断当前存储的点对数目n是否达到随机选点数N，若是则进入步骤S6，否则返回步骤S3；

S6、使用奇异分解求解出已经存储的N对点对(y_n,p_n)的旋转和平移变换关系，得到新的模型参数R_k和t_k；

S7、根据公式得到误差error_k，判断其是否满足收敛条件，若收敛则算法结束，否则根据下降步长S_d减少距离阈值d_M，替换模型参数，返回步骤S2。

2.根据权利要求1所述的点云配准算法，其特征在于，所述步骤S4中vanilla-R原则具体为：

当前点对(a_j,m_i)的距离应小于当前距离阈值d_M，如果不满足则返回步骤S3；在满足前一个条件时，在点云M中去搜索a_j的最近点m′_i，并判断m_i与m′_i的距离是否小于互邻距离阈值d_R。

3.根据权利要求1所述的点云配准算法，其特征在于，所述步骤S6包括以下分步骤：

S61、计算出两个点云的几何中心，即：

S62、计算出矩阵

S63、对W矩阵进行奇异值分解，求出U和V矩阵；

S64、计算出旋转矩阵R_k＝USV^T，其中S是一个3阶的对角矩阵，对角元素为{1 1 det U·det V}，det U和det V表示U和V矩阵的行列式；

S65、计算出平移向量t_k＝y_c-R_kp_c。

4.根据权利要求1所述的点云配准算法，其特征在于，所述步骤S7中的收敛条件为：

首先，要求误差error_k必须低于人为设置的阈值θ_er，θ_er＝αN；再判断误差error_k是否收敛，即是否满足|error_k-error_k-1|＜ε_er，ε_er＝βN；为了防止随机选点的影响，设置了一个收敛计数n_cvg，当以上条件出现n_cvg次数时，才会认为满足要求的收敛已经达到。