CN110033409B - 一种迭代最近点刚性注册方法及系统 - Google Patents

Abstract

本发明公开了一种迭代最近点刚性注册方法及系统，方法包括：基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点；固定对应点，求解刚性变换矩阵；判断刚性变换矩阵是否收敛，若是，则将参数值减小，直至达到参数值下界时终止迭代；若否，则：判断目标函数值是否增加，若是，则：采用加速之前的迭代值重新查找对应点，若否，则：根据对应关系求解变换，根据历史迭代值采用安德森加速方法进行加速，得到加速解。本发明能够有效的解决迭代最近点刚性注册时，求解速度慢、精度不高、依赖于参数等缺点。

Description

一种迭代最近点刚性注册方法及系统

技术领域

本发明涉及点云刚性注册技术领域，尤其涉及一种迭代最近点刚性注册方法及系统。

背景技术

由于三维点云扫描技术的飞速发展，通过扫描得到点云数据来重建三维物体或场景进行移动机器人定位、物体表面重建以及形状匹配等方面应用越来越广泛。由于光的线性传播特性，扫描设备每次只能测量得到物体局部坐标系下的部分表面，并且可能出现平移错位或旋转错位，因此为了得到物体完整表面的点云数据，需要对这些局部点云数据进行整合和配准。

点云注册(点云配准)是寻找一种三位空间中的刚性变换使得在不同视角下的三维坐标点共同部分能够正确的匹配和拼接。如何快速精确的对这些大规模的散乱点云进行配准仍然是现在的研究热点，目前的点云注册分为手动注册、依赖仪器的注册和自动注册。自动注册一般分为初始注册(全局注册)和精确注册(局部注册)，初始注册是为了缩小点云之间的旋转和平移错位以及提高精确注册的效率和趋向，精确注册则是为了是两个点云之间的配准误差达到最小。

目前的精确注册一般采用传统的迭代最近点刚性注册算法，该方法通过寻找一个点云在另一个点云中的最近点作为对应匹配点，并且计算对应匹配点之间的变换参数以满足给定的收敛精度，最终求得两个点云之间的平移和旋转参数，来完成配准过程。由于两个用于配准的点云数据大多是部分重叠，并且由于扫描设备本身限制，表面存在噪声，传统的迭代最近点刚性注册很容易受到噪声和初值的影响而陷入局部最优解，很多算法在此基础上进行了改进，比如点到面的度量方法，排除错误对应点对，稀疏的度量函数等方法，但是仍存在求解速度慢、精度不高、依赖于参数等缺点。

因此，如何有效的解决迭代最近点刚性注册时，求解速度慢、精度不高、依赖于参数等缺点，是一项亟待解决的问题。

发明内容

有鉴于此，本发明提供了一种迭代最近点刚性注册方法，能够有效的解决迭代最近点刚性注册时，求解速度慢、精度不高、依赖于参数等缺点。

本发明提供了一种迭代最近点刚性注册方法，包括：

基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点；

固定对应点，求解刚性变换矩阵；

判断所述刚性变换矩阵是否收敛，若是，则将参数值减小，直至达到参数值下界时终止迭代；若否，则：

判断目标函数值是否增加，若是，则：采用加速之前的迭代值重新查找对应点，若否，则：

根据对应关系求解变换；

根据历史迭代值采用安德森加速方法进行加速，得到加速解。

优选地，所述基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点，包括：

基于输入的初始点云和目标点云，通过对所述目标点云建立分割k维数据空间的数据结构进行最近邻搜索，得到匹配的对应点。

优选地，所述固定对应点，求解刚性变换矩阵，包括：

采用函数

作为目标函数

求解刚性变换矩阵；其中，x，y分别为初始点云和目标点云表面，

T表示待求解的刚性变换矩阵，

表示x_i的对应点，其中，ψ(x)＝1-exp(-x²/2υ²)，I_SE(T)表示指示函数：

优选地，所述判断所述刚性变换矩阵是否收敛包括：

判断残差是否减小为0。

优选地，所述根据历史迭代值采用安德森加速方法进行加速，得到加速解，包括：

对于每步迭代值，将其对应的李代数作为其表示形式，采用安德森加速方法进行加速，得到加速解。

一种迭代最近点刚性注册系统，包括：

搜索模块，用于基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点；

求解模块，用于固定对应点，求解刚性变换矩阵；

第一判断模块，用于判断所述刚性变换矩阵是否收敛；

参数调整模块，用于所述刚性变换矩阵收敛时，将参数值减小，直至达到参数值下界时终止迭代；

第二判断模块，用于所述刚性变换矩阵未收敛时，判断目标函数值是否增加；

迭代值确定模块，用于当目标函数值增加时，采用加速之前的迭代值重新查找对应点；

求解模块，用于当目标函数值未增加时，根据对应关系求解变换；

加速模块，用于根据历史迭代值采用安德森加速方法进行加速，得到加速解。

优选地，所述搜索模块在执行基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点时，具体用于：

基于输入的初始点云和目标点云，通过对所述目标点云建立分割k维数据空间的数据结构进行最近邻搜索，得到匹配的对应点。

优选地，所述求解模块在执行固定对应点，求解刚性变换矩阵时，具体用于：

采用函数

作为目标函数

求解刚性变换矩阵；其中，x，y分别为初始点云和目标点云表面，

T表示待求解的刚性变换矩阵，

表示x_i的对应点，其中，ψ(x)＝1-exp(-x²/2υ²)，I_SE(T)表示指示函数：

优选地，所述第一判断模块在执行判断所述刚性变换矩阵是否收敛时，具体用于：

判断残差是否减小为0。

优选地，所述加速模块在执行根据历史迭代值采用安德森加速方法进行加速，得到加速解时，具体用于：

对于每步迭代值，将其对应的李代数作为其表示形式，采用安德森加速方法进行加速，得到加速解。

综上所述，本发明公开了一种迭代最近点刚性注册方法，当需要实现迭代最近点刚性注册时，首先基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点，然后固定对应点，求解刚性变换矩阵，判断刚性变换矩阵是否收敛，若是，则将参数值减小，直至达到参数值下界时终止迭代；若否，则：判断目标函数值是否增加，若是，则：采用加速之前的迭代值重新查找对应点，若否，则：根据对应关系求解变换，根据历史迭代值采用安德森加速方法进行加速，得到加速解。本发明能够有效的解决迭代最近点刚性注册时，求解速度慢、精度不高、依赖于参数等缺点。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明公开的一种迭代最近点刚性注册方法实施例1的方法流程图；

图2为本发明公开的一种迭代最近点刚性注册系统实施例1的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，为本发明公开的一种迭代最近点刚性注册方法实施例1的方法流程图，所述方法可以包括：

S101、基于输入的初始点云和目标点云，通过对目标点云建立分割k维数据空间的数据结构进行最近邻搜索，得到匹配的对应点；

由于传统的迭代最近点刚性算法存在易受异常值和噪声影响的问题，而改进的方法虽然可以有效减少噪声影响，但是求解速度很慢，不能实时优化，而且依赖于参数设置。为此，本发明公开的方法实施例提出采用Welsch函数作为目标函数，采用安德森加速对模型求解进行加速，并且动态调整参数的策略，有效改善传统的迭代最近点刚性算法。本发明公开的方法实施例将目标函数设为：

其中，x，y分别为初始点云和目标点云表面，

T表示待求解的刚性变换矩阵，

表示x_i的对应点，其中，ψ(x)＝1-exp(-x²/2υ²)，I_SE(T)表示指示函数：

具体求解过程首先对于输入的初始点云与目标点云，通过对于目标点云建立KD-tree数据结构进行最近邻搜索，得到匹配的对应点。

S102、固定对应点，求解刚性变换矩阵；

然后固定对应点，求解刚性变换矩阵T，即求解最小值问题：

具体的，为求解该问题，可以采用最大化-最小化算法：

(a)找到满足条件的一个上界函数为

其中，

(b)通过优化其上界函数，已达到优化原函数的结果，即求解问题：

其中，

S103、判断刚性变换矩阵是否收敛，若是，则进入S104，若否，则进入S105：

在交替进行上述(a)和(b)的过程中，判断刚性变换矩阵是否收敛。将上述步骤S101和步骤S102的过程视为一个固定点迭代(G(x)＝x)的问题，对于一个固定点问题，残差r(x)＝G(x)-x减小为0时达到收敛状态，安德森加速通过极小化历史m步迭代的残差来得到新的迭代值，令G^k＝G(x^k),r^k＝G^k-x^k，通过重心坐标插值，可以得到：

则通过极小化残差(求解最小二乘问题)可以得到：

则新的迭代值为：

x^k+1＝(1-β)x(α^*)+βG(x(α^*))。

其中，迭代值(刚性变换矩阵T)采用其李代数形式标识，即ξ＝log(T)作为迭代值x的表示形式。

S104、将参数值减小，直至达到参数值下界时终止迭代；

通过交替进行上述(a)和(b)的过程直到刚性变换矩阵T收敛。其中，其中参数υ初始值选为初始最近点中值的k倍，其中k可以取3，终止υ值选取为每个点云各自的中值边长。当固定一个参数υ达到收敛时，υ值减为原来的一半，继续迭代，直到达到参数值下界，终止迭代，输出刚性变换矩阵。

S105、判断目标函数值是否增加，若是，则进入S106，若否，则进入S107：

根据得到的新的迭代值判断该迭代值是否使得目标函数值增加。

S106、采用加速之前的迭代值重新查找对应点；

当得到的新的迭代值使得目标函数值增加时，抛弃这个值而采用为加速时的迭代值以保证目标函数值稳定减小。

S107、根据对应关系求解变换；

S108、对于每步迭代值，将其对应的李代数作为其表示形式，采用安德森加速方法进行加速，得到加速解。

综上所述，传统的迭代最近点刚性注册算法通过：1)找初始点云在目标点云中对应的最近点作为其对应点；2)根据对应点，极小化能量函数(初始点云经过变换后与目标点云逐点之间的欧式距离和)，得到该种对应关系下的最优变换。重复两个过程直到初始点云位置几乎不变。

本发明公开的方法实施例将上述两个过程看作一个固定点迭代的问题，采用适用于加速固定点问题的安德森加速方法来加速，通过使用历史迭代值极小化残差得到新的迭代值；

由于传统能量函数采用变换后的初始点云和目标点云逐点之间的L2范数作为距离度量，这样使得注册结果很容易受到噪声和异常值的影响，由于Welsch函数具有更加稀疏的特性，能够使得距离较远的对应点对对目标函数整体的影响较小，因此发明公开的方法实施例采用Welsch函数作为逐点之间的距离度量；

对于采用Welsch函数为度量的目标函数，使用最大化-最小化(MM)算法来求解：1)找到当前目标函数的一个上界函数(在当前值处与原函数值相同，其他点处均大于原函数值)；2)极小化替代函数以达到极小化原目标函数的效果；

由于Welsch函数的能量函数需要设置参数，而不同规模的模型需要的参数值不同，寻找适合该模型的参数值比较麻烦，而且随着注册过程的进行，固定的参数不能达到很好的效果，因此本发明提出动态调整参数的方法来解决该问题。首先初始参数根据初始的对应点逐点距离的中值来选取，当迭代收敛时，将参数值减小，继续迭代，直到降到终止值及以下迭代终止，终止值根据两个点云各自的边长中值来选取。

由上述发明公开的方法实施例提供的技术方案可以看出：发明公开的方法实施例通过将原问题看作为固定点问题进行安德森加速，在不改变原有求解精度的基础上提高了求解速度；通过改变原有的L2范数的能量函数为更加稀疏的能量函数，使得模型不易受异常值及噪声影响；通过参数值的调整策略使得模型可以适合于任何数据而基本不需要人工调整参数。

如图2所示，为本发明公开的一种迭代最近点刚性注册系统实施例1的结构示意图，所述系统可以包括：

搜索模块201，用于基于输入的初始点云和目标点云，通过对目标点云建立分割k维数据空间的数据结构进行最近邻搜索，得到匹配的对应点；

由于传统的迭代最近点刚性算法存在易受异常值和噪声影响的问题，而改进的方法虽然可以有效减少噪声影响，但是求解速度很慢，不能实时优化，而且依赖于参数设置。为此，本发明系统实施例提出采用Welsch函数作为目标函数，采用安德森加速对模型求解进行加速，并且动态调整参数的策略，有效改善传统的迭代最近点刚性算法。本发明将目标函数设为：

其中，x，y分别为初始点云和目标点云表面，

T表示待求解的刚性变换矩阵，

表示x_i的对应点，其中，ψ(x)＝1-exp(-x²/2υ²)，I_SE(T)表示指示函数：

具体求解过程首先对于输入的初始点云与目标点云，通过对于目标点云建立KD-tree数据结构进行最近邻搜索，得到匹配的对应点。

求解模块202，用于固定对应点，求解刚性变换矩阵；

然后固定对应点，求解刚性变换矩阵T，即求解最小值问题：

具体的，为求解该问题，可以采用最大化-最小化算法：

(a)找到满足条件的一个上界函数为

其中，

(b)通过优化其上界函数，已达到优化原函数的结果，即求解问题：

其中，

第一判断模块203，用于判断刚性变换矩阵是否收敛；

在交替进行上述(a)和(b)的过程中，判断刚性变换矩阵是否收敛。将上述搜索模块的搜索过程和求解模块的求解过程视为一个固定点迭代(G(x)＝x)的问题，对于一个固定点问题，残差r(x)＝G(x)-x减小为0时达到收敛状态，安德森加速通过极小化历史m步迭代的残差来得到新的迭代值，令G^k＝G(x^k),r^k＝G^k-x^k，通过重心坐标插值，可以得到：

则通过极小化残差(求解最小二乘问题)可以得到：

则新的迭代值为：

x^k+1＝(1-β)x(α^*)+βG(x(α^*))。

其中，迭代值(刚性变换矩阵T)采用其李代数形式标识，即ξ＝log(T)作为迭代值x的表示形式。

参数调整模块204，用于刚性变换矩阵收敛时，将参数值减小，直至达到参数值下界时终止迭代；

通过交替进行上述(a)和(b)的过程直到刚性变换矩阵T收敛。其中，其中参数υ初始值选为初始最近点中值的k倍，其中k可以取3，终止υ值选取为每个点云各自的中值边长。当固定一个参数υ达到收敛时，υ值减为原来的一半，继续迭代，直到达到参数值下界，终止迭代，通过输出模块输出刚性变换矩阵。

第二判断模块205，用于刚性变换矩阵未收敛时，判断目标函数值是否增加；

根据得到的新的迭代值判断该迭代值是否使得目标函数值增加。

迭代值确定模块206，用于当目标函数值增加时，采用加速之前的迭代值重新查找对应点；

当得到的新的迭代值使得目标函数值增加时，抛弃这个值而采用为加速时的迭代值以保证目标函数值稳定减小。

求解模块207，用于当目标函数值未增加时，根据对应关系求解变换；

加速模块208，用于对于每步迭代值，将其对应的李代数作为其表示形式，采用安德森加速方法进行加速，得到加速解。

综上所述，传统的迭代最近点刚性注册算法通过：1)找初始点云在目标点云中对应的最近点作为其对应点；2)根据对应点，极小化能量函数(初始点云经过变换后与目标点云逐点之间的欧式距离和)，得到该种对应关系下的最优变换。重复两个过程直到初始点云位置几乎不变。

本发明系统实施例将上述两个过程看作一个固定点迭代的问题，采用适用于加速固定点问题的安德森加速方法来加速，通过使用历史迭代值极小化残差得到新的迭代值；

由于传统能量函数采用变换后的初始点云和目标点云逐点之间的L2范数作为距离度量，这样使得注册结果很容易受到噪声和异常值的影响，由于Welsch函数具有更加稀疏的特性，能够使得距离较远的对应点对对目标函数整体的影响较小，因此本发明系统实施例采用Welsch函数作为逐点之间的距离度量；

对于采用Welsch函数为度量的目标函数，使用最大化-最小化(MM)算法来求解：1)找到当前目标函数的一个上界函数(在当前值处与原函数值相同，其他点处均大于原函数值)；2)极小化替代函数以达到极小化原目标函数的效果；

由于Welsch函数的能量函数需要设置参数，而不同规模的模型需要的参数值不同，寻找适合该模型的参数值比较麻烦，而且随着注册过程的进行，固定的参数不能达到很好的效果，因此本发明系统实施例提出动态调整参数的方法来解决该问题。首先初始参数根据初始的对应点逐点距离的中值来选取，当迭代收敛时，将参数值减小，继续迭代，直到降到终止值及以下迭代终止，终止值根据两个点云各自的边长中值来选取。

由上述本发明系统实施例提供的技术方案可以看出：本发明系统实施例通过将原问题看作为固定点问题进行安德森加速，在不改变原有求解精度的基础上提高了求解速度；通过改变原有的L2范数的能量函数为更加稀疏的能量函数，使得模型不易受异常值及噪声影响；通过参数值的调整策略使得模型可以适合于任何数据而基本不需要人工调整参数。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (8)

1.一种迭代最近点刚性注册方法，其特征在于，包括：

基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点；

固定对应点，求解刚性变换矩阵；

判断所述刚性变换矩阵是否收敛，若是，则将参数值减小，直至达到参数值下界时终止迭代；若否，则：

判断目标函数值是否增加，若是，则：采用加速之前的迭代值重新查找对应点，若否，则：

根据对应关系求解变换，根据历史迭代值采用安德森加速方法进行加速，得到加速解；

所述固定对应点，求解刚性变换矩阵，包括：

采用目标函数

求解刚性变换矩阵；

其中，

分别为初始点云和目标点云曲面，x_i表示

上的采样点，

表示采样点x_i的集合，|X|表示采样点集合X中元素的个数，y_i表示

上的采样点，

表示采样点y_i的集合，|Y|表示采样点集合Y中元素的个数，

为x_i在Y集合中的对应点，其中ρ_i是对应点的标号，T表示待求解的刚性变换矩阵，SE(d)表示d维刚性变换空间，I_SE(d)(T)表示指示函数，

υ是可改变的参数。

2.根据权利要求1所述的方法，其特征在于，所述基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点，包括：

基于输入的初始点云和目标点云，通过对所述目标点云建立分割k维数据空间的数据结构进行最近邻搜索，得到匹配的对应点。

3.根据权利要求1所述的方法，其特征在于，所述判断所述刚性变换矩阵是否收敛包括：

判断残差是否减小为0。

4.根据权利要求3所述的方法，其特征在于，所述根据历史迭代值采用安德森加速方法进行加速，得到加速解，包括：

对于每步迭代值，将其对应的李代数作为其表示形式，采用安德森加速方法进行加速，得到加速解。

5.一种迭代最近点刚性注册系统，其特征在于，包括：

搜索模块，用于基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点；

求解模块，用于固定对应点，求解刚性变换矩阵；

第一判断模块，用于判断所述刚性变换矩阵是否收敛；

参数调整模块，用于所述刚性变换矩阵收敛时，将参数值减小，直至达到参数值下界时终止迭代；

第二判断模块，用于所述刚性变换矩阵未收敛时，判断目标函数值是否增加；

迭代值确定模块，用于当目标函数值增加时，采用加速之前的迭代值重新查找对应点；

求解模块，用于当目标函数值未增加时，根据对应关系求解变换；

加速模块，用于根据历史迭代值采用安德森加速方法进行加速，得到加速解；

所述求解模块在执行固定对应点，求解刚性变换矩阵时，具体用于：

采用目标函数

求解刚性变换矩阵；

其中，

分别为初始点云和目标点云曲面，x_i表示

上的采样点，

表示采样点x_i的集合，|X|表示采样点集合X中元素的个数，y_i表示

上的采样点，

表示采样点y_i的集合，|Y|表示采样点集合Y中元素的个数，

为x_i在Y集合中的对应点，其中ρ_i是对应点的标号，T表示待求解的刚性变换矩阵，SE(d)表示d维刚性变换空间，I_SE(d)(T)表示指示函数，

υ是可改变的参数。

6.根据权利要求5所述的系统，其特征在于，所述搜索模块在执行基于输入的初始点云和目标点云，进行最近邻搜索，得到匹配的对应点时，具体用于：

基于输入的初始点云和目标点云，通过对所述目标点云建立分割k维数据空间的数据结构进行最近邻搜索，得到匹配的对应点。

7.根据权利要求5所述的系统，其特征在于，所述第一判断模块在执行判断所述刚性变换矩阵是否收敛时，具体用于：

判断残差是否减小为0。

8.根据权利要求7所述的系统，其特征在于，所述加速模块在执行根据历史迭代值采用安德森加速方法进行加速，得到加速解时，具体用于：

对于每步迭代值，将其对应的李代数作为其表示形式，采用安德森加速方法进行加速，得到加速解。

Images