一种基于抗差约束最小二乘算法的点云精确配准方法与流程

本发明属于计算机视觉领域，具体涉及一种基于抗差约束最小二乘(robustconstrainedleastsquares，简称rcls)算法的点云精确配准方法。

背景技术：

3d点云配准是计算机视觉领域的关键研究问题之一，在逆向工程、slam、图像处理和模式识别等方面具有重要应用。点云配准的目的是求解出同一坐标下不同姿态点云的变换矩阵，利用该矩阵实现多视扫描点云的精确配准，最终获取完整的3d数字模型。

针对3d点云配准，国内外学者进行了大量的研究，但大部分研究方法往往集中于前期的对应点对搜索，通过提取点云的相关特征如法向量，曲率等以提高对应点对搜索效率，然后通过svd法或四元数法等解析解的方法求旋转矩阵r和平移向量t，但是这些方法没有考虑点云数据中含有的粗差以及三角函数计算的繁琐，在一定程度上限制了点云配准的精度。

技术实现要素：

发明目的：为解决上述问题，本发明提供了一种基于抗差约束最小二乘算法的点云精确配准方法，能进行任意角度的点云配准，缩短配准时间，提高配准精度。

技术方案：本发明所述的一种基于抗差约束最小二乘算法的点云精确配准方法，包括以下步骤：

(1)获取经过去噪滤波处理的待配准点云和目标点云；

(2)通过pca算法对待配准点云和目标点云进行粗配准；

(3)利用kd-tree搜索待配准点云在目标点云中的对应点，设置搜索距离阈值η，删除错误的点对；

(4)根据点云的法向量特征，提取特征度较大的点；

(5)将得到的对应点对利用抗差约束最小二乘算法，将待配准点云通过旋转和平移配准到目标点云上，直至两点云的满足距离阈值δ(1e-6)。

进一步地，所述步骤(1)实现过程如下：

采用soa的离群噪声去除算法对两点云进行去噪滤波处理，设待处理的点云为p＝{pi∈r³∣i＝1,2,3,…,n}，n为待处理点云中的采样点个数，对其中的任意采样点pi，建立pi的k邻域，计算采样点pi到它的k邻域点的平均距离，之后判断平均距离，如果距离在设定范围内，则判定为主体点云，否则，判定为噪声点，然后移除噪声点。

进一步地，所述步骤(2)实现过程如下：

(21)计算待配准点云p和目标点云q的质心点坐标：

式中，n和m代表待配准点云p和目标点云q的点云数量，up和uq代表p，q的质心点坐标；

(22)计算两组点云的协方差：

covp＝(p-up)(p-up)^t

covq＝(q-uq)(q-uq)^t

式中，up和uq代表p，q的质心点坐标，cov表示协方差，t表示转置；

(23)通过协方差矩阵，分别求两组点云各自的特征向量，按特征值将特征向量从大到小排列，求出粗配准的旋转矩阵r和平移矩阵t，将两组点云形成的坐标轴统一到相同的坐标系下，完成粗配准。

进一步地，步骤(3)所述的搜索距离阈值η为所有对应点对距离平均值的1.5倍。

进一步地，所述步骤(4)实现过程如下：

(41)点云中某一点pi处法向量的变化程度即其特征度fi，定义为其法向量与其k近邻点夹角的算术平均值：

其中，θij为点pi的法向量与其近邻点pj的法向量的夹角；

(42)选取适当的阈值ε1，去掉点云中较为平坦的部分并保留fi>ε1的点，对于保留点中的任一pm，若其满足：

pmf(pm)＝max[f(pm1),f(pm2),…,f(pmk)]

将pm作为特征点，其中f(pm1),f(pm2),…,f(pmk)为点pm的k近邻点的特征度；

(43)设两点云为p和q，其中p为待配准点云，q为目标点云，分别对两个点云进行特征点提取，得到p的特征点集为pt＝{ptl,pt2,pt3…,ptn′}，q的特征点集为qt＝{qtl,qt2,qt3,…,qtm′}，其中n′和m′分别为p和q的特征点的个数。

进一步地，所述步骤(5)实现过程如下：

(51)对待配准点云和目标点云的坐标进行重心化处理，并建立重心化后的误差方程；

(52)使用间接平差的基本公式得到参数近似值，作为待估参数初始值；

(53)得到5个约束条件，组成约束条件方程，由(52)得到的参数的初始值可以得到wx和c矩阵；

(54)使用附有限制条件的间接平差更新参数值,求出旋转矩阵r；

(55)根据参数的大小来判断是否满足收敛条件，当待估参数的二范数就可认为参数满足收敛，更新参数估值；若不满足条件，重复迭代的过程；

(56)对平移参数进行还原，根据求得的转换参数计算待配准点云在新坐标系中的坐标。

有益效果：与现有技术相比，本发明的有益效果：有效解决了原始点云数据精度不高，数据量大的情况，大幅缩短配准时间，与传统解析解的方法相比，初始值获取较容易，无需用复杂的三角函数计算，避免在配准过程中陷入局部最优解，得到更加精确的配准结果。

附图说明

图1为本发明所述的构建方法的流程示意图；

图2为点云kd-tree的建立示意图；

图3为不同区域的法向量示意图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

本发明提供了一种基于抗差约束最小二乘算法的点云精确配准方法，该方法首先对经过去噪滤波处理后待配准点云和目标点云利用pca算法进行粗配准，再利用kd-tree在待配准点云与目标点云中搜索对应点对。其次，根据点云的法向量特征，保留特征度较大的点对；最后将得到的对应点对根据约束最小二乘抗差法求解旋转矩阵r和平移向量t，将待配准点云通过旋转和平移配准到目标点云上，直到两点云距离差值的绝对值最小，从而实现点云的精确配准。如图1所示，具体包括以下步骤：

步骤一，获取经过去噪滤波处理的待配准点云和目标点云。

采用statisticaloutlierremoval(soa)的离群噪声去除算法对两点云进行去噪滤波处理，该方法的主要原理是待处理点云为p＝{pi∈r³∣i＝1,2,3,…,n}，n为待处理点云中的采样点个数，对其中的任意采样点pi，首先建立pi的k邻域，然后计算采样点pi到它的k邻域点的平均距离，之后判断平均距离，如果距离在设定范围内(定义的方差和全局距离平均值)，则判定为主体点云，否则，判定为噪声点，然后移除这些噪声点并扩散到点的邻域中去。从而能够使曲面更加光顺。

sor去噪时需要设置两个阈值，一个是邻域点的数量k，另一个是标准差的倍数n，如下式：

其中，表示平均距离，σ表示标准差，当距离超过dmax时会判定为噪声点被删除。

步骤二，通过pca算法对待配准点云和目标点云进行粗配准。

首先计算两组点云p，q的质心点坐标：

式中，n和m代表待配准点云p和目标点云q的点云数量，up和uq代表p，q的质心点坐标。

其次计算两组点云的协方差：

covp＝(p-up)(p-up)^t

covq＝(q-uq)(q-uq)^t

式中，up和uq代表p，q的质心点坐标，cov表示协方差，t表示转置。

通过协方差矩阵，分别求两组点云各自的特征向量，按特征值将特征向量从大到小排列，求出旋转矩阵r和平移矩阵t，将两组点云形成的坐标轴统一到相同的坐标系下，完成粗配准相关过程。

步骤三，利用kd-tree搜索待配准点云在目标点云中的对应点，设置搜索距离阈值η(η为所有对应点对距离平均值的1.5倍)，删除错误的点对。

kd-tree简称k维树，是一种空间划分的数据结构。常被用于高维空间中的搜索，比如范围搜索和最近邻搜索。kd-tree是二进制空间划分树的一种特殊情况，在三维点云中，kd-tree的维度是3。kd-tree的每一级在指定维度上分开所有的子节点。在树的根部所有子节点是以第一个指定的维度上被分开，树的每一级都在下一个维度上分开，所有其他的维度用完之后就回到第一个维度，所以，kd-tree对于区间和近邻搜索效率很高。

三维点云数据kd-tree的生成过程先依据点云在某个方向上的方差值，按方差值由大到小选取yz平面、xz平面或xy平面为分割面。重复分割过程，直至分割后的区域内不包含任何点，结束分割。点云kd-tree建立如图2所示。

步骤四，根据点云的法向量特征，提取特征度较大的点。

定义点云中某一点pi处法向量的变化程度即其特征度fi为其法向量与其k近邻点夹角的算术平均值：

式中θij为点pi的法向量与其近邻点pj的法向量的夹角。

根据这个定义，特征度越大的点表示该区域起伏变化越大，所以可以利用此特征度来提取点云中的特征点。选取适当的阈值ε1，去掉点云中较为平坦的部分并保留fi>ε1的点，对于保留点中的任一pm，若其满足：

pmf(pm)＝max[f(pm1),f(pm2),…,f(pmk)]

将pm作为特征点，其中f(pm1),f(pm2),…,f(pmk)为点pm的k近邻点的特征度。设两点云为p和q，其中p为待配准点云，q为目标点云。分别对两个点云进行特征点提取，得到p的特征点集为pt＝{ptl,pt2,pt3…,ptn′}，q的特征点集为qt＝{qtl,qt2,qt3,…,qtm′}，其中n′和m′分别为p和q的特征点的个数。

由于不同区域的法向量不同，如图3所示，根据点云的法向量特征，提取特征度较大的点，由于fi多数情况下主要分布在0°～60°间，本发明将fi分成[0°，20°]、(20°，40°]、(40°，60°]和(60°，180°]等4个间隔，只保留前三个间隔的点。

步骤五，点云配准的实质是将待配准点云和目标点云通过三维坐标转换到同一坐标系下。抗差约束最小二乘算法关于参数是线性的，可采用间接平差获得比较理想的初值，利用该初值可以更好的求解转换参数。

在约束最小二乘算法中引入等价权抗差原理，采用抗差估计算法进行参数估计，即采用iggⅲ权函数进行抗差处理。抗差约束最小二乘算法是基于五种约束条件的十二参数模型观测方程，相比于传统的非线性七参数模型，十二参数模型观测方程关于参数是线性的，可以采用间接平差获得比较理想的初值，并且十二参数模型避免了由三角函数表示旋转矩阵的复杂运算。具体包括以下步骤：

(1)对待配准点云和目标点云的坐标进行重心化处理，并建立重心化后的误差方程。

设步骤四搜索得到的对应点对为pi和qi，pi和qi表示在待配准点云和目标点云中搜索的对应点。

通用三维坐标转换中公共点的数学模型如下:

式中，[xpiypizpi]^t和[xqiyqizqi]^t分别代表第i个点在待配准点云pi坐标和目标点云qi坐标系统下的坐标；[exqieyqiezqi]^t代表目标点云坐标下的随机误差向量。t＝[△x△y△z]^t表示平移向量，r是一个3×3的旋转矩阵。

在对待配准点云坐标pi(xpi,ypi,zpi)和目标点云坐标qi(xqi,yqi,zqi)按照下式进行重心化处理：

式中，(x'pi,y'pi,z'pi)和(x'qi,y'qi,z'qi)表示重心化后的坐标，(xpg,ypg,zpg)和(xqg,yqg,zqg)分别表示待配准点云坐标pi和目标点云坐标qi的重心坐标，在确定后可以作为参数来处理。

式中，n表示搜索的点云中所含点的个数，由上式可以建立误差方程如下：

v′i＝bix′-li

式中，v′i表示改正数，bi表示系数项，x′表示待定参数近似值的改正数，li表示观测向量。

上式中：

x′＝(△x'△y'△z'(vec(r^t))^t)^t

式中，t'＝[△x'△y'△z']^t，(x'pi,y'pi,z'pi)和(x'qi,y'qi,z'qi)表示重心化后的坐标，“vec”代表列向量化算子，是将矩阵的各列按照从左往右的顺序堆叠到一起。

(2)使用间接平差的基本公式得到参数近似值，作为待估参数初始值

(3)抗差约束最小二乘算法的5个约束条件，组成约束条件方程，由参数的初始值可以得到wx和c矩阵。

抗差约束最小二乘算法五个约束条件为：

式中，ξ11，ξ12，ξ13，ξ21，ξ22，ξ23，ξ31，ξ32，ξ33表示约束参数。

对五个约束条件进行线性化，并写成标准形式为：

cx+wx＝0

式中，c表示限制条件方程的系数矩阵，x表示待定参数近似值的改正数，wx表示限制条件方程的闭合差常数向量。

其中：

(4)使用附有限制条件的间接平差求解更新参数值。

其平差模型如下：

式中，v表示观测误差，l表示误差方程常数项，b表示观测值误差方程的系数矩阵，表示待定参数近似值的改正数，c表示限制条件方程的系数矩阵，wx表示限制条件方程的闭合差常数向量，d表示协方差阵，σ0表示单位权方差，q与p分别表示对应的协因数阵和观测权阵。

组成法方程为：

式中，nbb表示法方程的系数矩阵，表示待定参数近似值的改正数，c表示限制条件方程的系数矩阵，ks表示限制条件方程的联系数向量，w表示闭合差向量。

其中：

方程的解为：

根据方程的解求出ξ11，ξ12，ξ13，ξ21，ξ22，ξ23，ξ31，ξ32，ξ33，即求出旋转矩阵r：

(5)根据参数的大小来判断是否满足收敛条件，当待估参数的二范数就可认为参数满足收敛，更新参数估值；若不满足条件，重复迭代的过程；

(6)对平移参数进行还原，根据求得的转换参数计算待配准点云p在新坐标系中的坐标。

又因为重心化后两套坐标系统之间的参数，使得法方程病态性被大大削弱。相对于未重心化的两套坐标，发生改变的只有平移向量t＝[△x△y△z]^t

式中，[xqgyqgzqg]^t，[xpgypgzpg]^t分别表示目标点云坐标qi和待配准点云坐标pi重心化后的坐标，分别表示目标点云坐标qi和待配准点云坐标pi的准确坐标，又根据t'＝[△x'△y'△z']^t，可还原出平移向量t＝[△x△y△z]^t:

在实际应用中，通过使用约束最小二乘方法可以进行任意角度三维坐标转换。

抗差估计，是直接将具有一定抗差性的估计准则应用于观测数据，进行参数估计，本发明采用计算等价权阵p的iggⅲ权函数进行抗差处理。对于iggiii的权因子函数，对其求倒数便可获得对应的协因数因子函数：

式中，表示观测值第i点的标准化残差，rii表示观测值第i点的权因子，k0和k1均为常数，k0一般取1～1.5，k1一般取3左右。

由于iggiii函数的第三段为0，对应协因数因子的理论值应为无穷，为了方便实际计算，用一个大数(10¹⁰)代替，这样在数值计算上完全能够满足要求。

抗差估计的加入，可以有效地将点云进行分层定权处理，通过多次迭代过程，有效降低淘汰段的小粗差对点云配准的影响。使得约束最小二乘算法的转换精度更高，有效避免了粗差对数据的影响。将步骤四得到的对应点对利用抗差约束最小二乘算法求解旋转矩阵r和平移向量t，将待配准点云通过旋转和平移配准到目标点云上，直至两点云的距离满足距离阈值δ(δ一般取值1e-6)。若不满足，则将转换的待配准点云继续执行步骤三至步骤五，满足阈值δ后，输出结果。