CN103236064B - 一种基于法向量的点云自动配准方法 - Google Patents

Abstract

一种基于法向量的点云自动配准方法，处理对象为两幅及两幅以上相互间有重叠部分的三维点云数据，处理步骤为：(1)根据点云局部法向量的变化选取特征点；(2)设计一种直方图特征量对获得的每个特征点进行特征描述；(3)通过比较特征点的直方图特征向量获得初始匹配点对；(4)运用刚性距离约束条件结合RANSAC算法获取精确匹配点对，并利用四元素法计算得到初始配准参数；(5)采用改进的ICP算法对点云精确配准。按照上述步骤可对点云进行自动配准，本发明提出的特征描述简单且辨识度高，同时具有较高的鲁棒性，配准精度和速度都有一定的提高。

Description

一种基于法向量的点云自动配准方法

技术领域

本发明属于三维信息重构的领域，特别涉及一种点云自动配准方法。

背景技术

物体表面三维重构一直是机器视觉领域研究的一个重要课题。被测物体表面点云数据可以由光学扫描仪快速获取，但由于光的线性传播特性，被测物体表面的完整数据需要在多个视角下多次测量获得，这样得到的数据便不在同一个坐标系下，所以为了获得物体的完整模型，需要对各个视角得到的数据进行坐标变换，最终合并到统一的坐标系中，这就是点云配准。点云配准技术在机器人导航、逆向工程、物体表面形状检测和虚拟现实等众多领域有着广泛的应用。

根据待配准深度图的输入和输出结果的不同，现有的深度图配准方法可以大致分为两大类：粗配准和精配准。粗配准是指在没有任何先验知识的情况下，找到一组近似的坐标变换关系，将两个视点下的深度图统一到同一个坐标系中。由于配准之前没有深度图之间相对位置关系的任何信息，因此粗配准算法通常围绕如何建立模型相同位置处的两幅深度图之间对应元素的匹配关系展开，众多学者对此提出了不同的方法。这些方法按对应元素不同可以分为点-点对应、线-线对应和面-面对应以及体-体对应；若按匹配方式不同又可以分为基于欧氏距离相等关系和基于描述子相等关系；按匹配搜索范围可分为非特征匹配与特征匹配。然而由于测量模型的多样性和复杂性，粗配准算法往往依赖于一些具体的应用，对粗配准算法进一步研究的方向在于提高粗配准的精度、效率以及鲁棒性和适用性。粗配准的结果通常仅提供了深度图之间精确坐标变换的一个近似值，从而使得两幅深度图的重叠区域具有一定程度的贴合。然而这种近似的位置变换使得深度图重叠区域很难得到精确的贴合，往往会存在一些交错和分层现象，不利于后续深度图数据的融合，因此需要对深度图的位置进行进一步的调整以提高深度图的配准精度，这个过程称为精配准。精配准是在已通过粗配准获得了深度图之间坐标变换的近似值的基础上，通过不断迭代最小化两深度图上对应点之间的距离来实现更加精确的配准变换，其代表算法为经典的ICP算法。

发明内容

发明目的：提出一种基于法向量的点云自动配准方法，提高点云自动配准中配准的精度和速度，同时具有较高的鲁棒性。

技术方案：一种基于法向量的点云自动配准方法，包括以下步骤：

步骤1：基于法向量信息获取待匹配两个点云中的特征点集,具体步骤如下：

步骤1.1：采用三维扫描仪获取带有法向量信息的多视角点云数据,且相邻视角获得的点云间有重叠部分，定义点云中重叠部分的某一点p_i的特征度f_i为点p_i的法向量与其k个近邻点法向量夹角的算术平均值：

$f_i=\frac{1}{k}{\mathrm{\Σ}}_{j=1}^k{\mathrm{\θ}}_{\mathrm{ij}}$ （1）式

其中某一点p_i的k个近邻点是指与点p_i欧氏距离最近的k个点，θ_ij为点p_i的法向量与其近邻点p_j的法向量的夹角,k为5≤k≤20的自然数；

步骤1.2：选取阈值ε₁，去掉点云中f_i≤ε₁的平坦部分，保留f_i＞ε₁的点，对于保留点中的任一点p_m，若其满足：

f(p_m)＝max(f(p_m1),f(p_m2),…,f(p_mk))（2）式

则将p_m作为特征点,其中f(p_m1),f(p_m2),…,f(p_mk)为点p_m的k个近邻点的特征度，其中阈值ε₁取值范围为5°≤ε₁≤10°；

步骤1.3：利用步骤1.1和步骤1.2所述的特征点提取方法，分别对两个点云进行特征点提取，设待配准的两个点云数据分别为点集P和点集Q，其中点集Q为参考点云数据，得到待配准点云的特征点集为Pt＝{pt₁,pt₂,pt₃,…pt_m′}，参考点云的特征点集为Qt＝{qt₁,qt₂,qt₃,…qt_n′}，其中m′和n′分别为P和Q的特征点的个数；

步骤2：建立特征点集的直方图特征描述，方法如下：

步骤2.1：对于特征点集Pt中的每个点pt_i，在点集P中以pt_i为原点，半径为γ的球域内的点作为pt_i的邻近点，标记为N(pt_i)，其中γ取值范围为点云的平均点间距离的5～10倍；

步骤2.2：根据点pt_i与邻近点N(pt_i)之间的几何关系，建立三种特征描述：

f₁＝acos<n_i,v_k>（3）式

f₂＝<n_i,(s_k-pt_i)>（4）式

f₃＝‖s_k-pt_i‖（5）式

其中n_i为点pt_i的法向量，v_k为pt_i邻近某个点N(pt_i)的法向量，s_k为pt_i某个邻近点N(pt_i)的三维坐标；

其中，式（3）中特征值f₁是点集Pt中一点pt_i的法向量与其邻近一点N(pt_i)的法向量的夹角，根据夹角大小将其分成[0,20]、(20,40]、(40,60]和(60,180]4个间隔；式（4）中特征值f₂是两个向量点积，其中一个是点pt_i的法向量，另一个是该点pt_i与其邻近一点N(pt_i)之间的点间向量，根据f₂是否大于0，将f₂分成2个间隔；式（5）中特征值f₃是一点pt_i与其邻近点中一点N(pt_i)间的欧氏距离，根据阈值将其分成2个间隔；根据这三个特征值的间隔分类，我们可建立一个间隔数为4×2×2＝16的直方图，对应地得到一个16维的特征向量，其中γ取值范围为点云的平均点间距离的5～10倍；

步骤2.3：根据所述步骤2.2中三个特征值f₁、f₂、f₃，定义idex＝k₁+k₂·4+k₃·4·2；

若特征值f₁落入[0,20]间隔中则对应地将k₁记为1，若特征值f₁落入(20,40]间隔中则对应地将k₁记为2，若特征值f₁落入(40,60]间隔中则对应地将k₁记为3，若特征值f₁落入(60,180]间隔中则对应地将k₁记为4；若特征值f₂的值小于0，则将k₂记为1，否则记为0；若特征值f₃的值大于则将k₃记为1，否则记为0；

根据idex的值确定N(pt_i)中一点属于直方图中的间隔位置，遍历一个点pt_i的所有邻近点N(pt_i)，得到落入每个间隔中的邻近点N(pt_i)的数量，以N(pt_i)中落入每个间隔中的点数占其总数的百分比作为对应间隔的值，记此值为特征向量h1_i；

步骤2.4：根据所述步骤2.1至2.3，建立点集Pt和Qt中每个点的特征向量，得到点集Pt和Qt的特征向量集h1和h2；

步骤3：采取特征向量间的欧氏距离作为比较准则，比较所述步骤2中获得的特征向量集h1和h2中的特征向量，在点集Qt中为点集Pt中每个点找到其初始匹配点：选取阈值ε₂，特征向量的距离小于ε₂时，该特征向量所对应点集Pt和Qt中的点作为初始匹配点对，记为：

$\mathrm{Matchdots}=\left\{(s_i^1,s_i^2)\right|s_i^1\&Element;\mathrm{Pt},s_i^2\&Element;\mathrm{Qt},i=\mathrm{1,2,3}\&CenterDot;\&CenterDot;\&CenterDot;\mathrm{num}\left(\mathrm{Matchdots}\right)\}$

其中num(Matchdots)是初始匹配点对的数量，ε₂取0.05；

步骤4：结合刚性距离约束条件，使用一种自适应的RANSAC算法获取精确匹配点对，具体步骤如下：

步骤4.1：Matchdots中的任意两个点对和如果是正确的匹配点对，那么根据刚体变换的距离不变性得到：其中dist表示两点间的距离；选取ε₃＞0，对每个点对计算Matchdots中除点对以外与该点对符合刚性距离约束的点对的数目num_i，若Matchdots中一个点对满足下式：

$\frac{|\mathrm{dist}(s_i^1,s_j^1)-\mathrm{dist}(s_i^2,s_j^2)|}{\mathrm{dist}(s_i^1,s_j^1)+\mathrm{dist}(s_i^2,s_j^2)}<{\mathrm{\&epsiv;}}_3$ （6）式

则将点对记为相对于点对的符合距离约束的点对，同时将的num_i值加1，其中ε₃取0.02；

步骤4.2：对Matchdots中每个点对根据所述步骤4.1计算num_i值，然后按照num_i值从大到小对Matchdots中点对进行排序，选取前N个点对，N为N≤num(Matchdots)的自然数；使用RANSAC算法对这N个点对检验是否为正确匹配点对，具体检验方法为：

步骤4.2.1：从这N个点对中随机选取3个点对作为一个样本；

步骤4.2.2：假设这3个点对为正确匹配点对，计算刚体变换矩阵T；

步骤4.2.3：判断其余N-3个点对在此刚体变换矩阵T下是否为正确匹配：

如果小于预定义的阈值ε₄，则认为是正确的匹配点对，记为内点；如果大于等于预定义的阈值ε₄，则认为是错误的匹配点对，记为外点；所有内点组成本次采样的一致集；其中，ε₄取值范围为点云的平均点间距离的2倍；

步骤4.2.4：重复所述步骤4.2.1至步骤4.2.3，直到N个点中所有3点组合的样本都遍历了，循环次数达到采样次数上限后，将内点数目最多的刚体变换矩阵作为正确的刚体变换矩阵，据此变换矩阵求得初始匹配点对中的内点作为最终的精确匹配点对，记为：

Matchdots1＝{(m_i,m′_i)|m_i∈P,m′_i∈Q,i＝1,2,3…n}

其中n为精确匹配点对的数目；

步骤5：利用四元素法计算初始配准参数，具体步骤如下：

步骤5.1：计算点集{m_i|m_i∈P，i＝1,2,...,n}的质心μ和{m′_i|m′_i∈Q，i＝1,2,...,n}的质心μ′：

$\left\{\begin{array}{c}\mathrm{\&mu;}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{m}_i\\ {\mathrm{\&mu;}}^{\&prime;}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{m^{\&prime;}}_i\end{array}\right.$ （7）式

式（7）中，n为精确匹配点对的数目；

步骤5.2：将点集{m_i|m_i∈P，i＝1,2,...,n}和{m′_i|m′_i∈Q，i＝1,2,...,n}做相对于质心μ、μ′的平移，设平移后的点集分别为{h_i}和{h′_i}：

$\left\{\begin{array}{c}{h}_i=m_i-\mathrm{\&mu;}\\ {h^{\&prime;}}_i={m^{\&prime;}}_i-{\mathrm{\&mu;}}^{\&prime;}\end{array}\right.$ （8）式

步骤5.3：根据平移后的所得点集{h_i}和{h′_i}计算相关矩阵K：

$K=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{h}_i{\left({h^{\&prime;}}_i\right)}^T$ （9）式

式（9）中，n为精确匹配点对的数目；

步骤5.4：由相关矩阵K中各元素K_ij(i,j＝1,2,3,4)构造出四维对称矩阵K_a：

$K_a=\left[\begin{array}{cccc}{K}_{11}+K_{22}+K_{33}& K_{32}-K_{23}& K_{13}-K_{31}& K_{21}-K_{12}\\ K_{32}-K_{23}& K_{11}-K_{22}-K_{33}& K_{12}+K_{21}& K_{31}+K_{13}\\ K_{13}-K_{31}& K_{12}+K_{21}& {-K}_{11}+K_{22}-K_{33}& K_{23}+K_{32}\\ K_{21}-K_{12}& K_{31}+K_{13}& K_{23}+K_{32}& {-K}_{11}-K_{22}+K_{33}\end{array}\right]$ （10）式

步骤5.5：计算K_a的最大特征根所对应的单位特征向量q：

$q={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$ （11）式

步骤5.6：由步骤5.5中计算所得单位特征向量q，计算旋转矩阵R：

$R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2+q_2^2-q_1^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2+q_3^2-q_1^2-q_2^2\end{array}\right]$ （12）式

步骤5.7：由步骤5.6计算所得旋转矩阵R，求解平移矢量T：

T＝μ′-Rμ（13）式

步骤6：采用改进的ICP算法对点云精确配准，具体步骤如下：

步骤6.1：利用步骤5中获得的配准参数R和T，根据式（14）将待配准点云P中的每个点p_i变换到参考点云Q所在的坐标系下得到点p_0i，将转换后得到的初始配准后的点云记为P₀；其中：

p_0i＝R·p_i+T（14）式

步骤6.2：令目标点集为P_m,参考点集为Q,取m＝0,利用改进的ICP算法进行精确配准：

步骤6.2.1：计算最近点：

对目标点集P_m中任一点p_mi,计算该点与参考点集Q中每个点间的欧氏距离,对于点集Q中与其欧氏距离最小的点q_j，选取阈值σ,若：两者间欧氏距离||p_mi,q_j||²＜σ，则将p_mi作为内点并记为并将q_j作为其对应匹配点并记为这样便得到一组内点匹配点对否则将其作为外点去除；其中当m＝0时,阈值σ取为点云Q点间平均距离的3倍；

对目标点集P_m中所有点作以上操作，最终得到内点匹配点集，记为： $M^m=\left\{(p_i^m,q_i^m)\right|p_i^m\&Element;P_m,q_i^m\&Element;Q,i=\mathrm{1,2,3}\&CenterDot;\&CenterDot;\&CenterDot;n_p\},$ 其中n_p为点对数目；

步骤6.2.2：利用步骤6.2.1中获得内点匹配点集来计算配准参数：

计算使得式（15）中err取得最小时的旋转矩阵R_m和平移矩阵T_m作为新的配准参数：

$\mathrm{err}=\frac{1}{n_p}\underset{i=1}{\overset{n_p}{\mathrm{\&Sigma;}}}{\left|\right|q_i^m-(R_m{p}_i^m+T_m)\left|\right|}^2$ （15）式

其中err为本次配准的误差，n_p为点对数目；

步骤6.2.3：将计算得到的配准参数用于点集P_m中的每个点p_mi得到新的点集P′

$p_i^{\&prime;}=R_m{p}_{\mathrm{mi}}+T_m$ （16）式

其中，为点集P′中的点；

步骤6.2.4：检验迭代终止的条件：

设定阈值τ=0.00001，当满足：σ-err＜τ，则迭代过程结束；否则令m＝m+1，σ＝err,P_m＝P′转步骤6.2.1；

迭代过程结束后得到的点集P′即为最终的精确配准结果。

有益效果：与其他基于特征的配准技术相比，本发明能对部分重叠的点云数据实现自动配准，点云无需任何预知信息且配准过程无需人工干预，初始配准效果良好，二次配准结果更加精确，达到多视角点云数据的配准要求，具有如下优点：

（1）本发明基于点云局部法向量的变化程度选取特征点集，而不是将所有点都用于点对匹配，节省了处理时间，同时去除了相对特征不明显的点，提高了点对匹配的准确率，避免太多相似点匹配造成的错误匹配点对的出现。

（2）设计多特征融合的直方图特征，提高特征描述的辨识度，且本特征的抗噪声干扰能力强且受点云拓扑结构影响小，从而有效提高了点对匹配的准确率。

（3）利用刚性距离约束与RANSAC算法相结合获取准确匹配点对，进一步利用改进的ICP算法迭代求取精确配准结果，算法快速有效。

附图说明

图1是本发明方法流程图；

图2是不同区域法向量变化的表征图；

图3是取不同的阈值ε₁和k值时获得的特征点集图；

图4是点云bunny上一个特征点的直方图特征图；

图5是点云bunny的两个视角的初始配准结果；

图6是点云bunny的两个视角的精确配准结果。

具体实施方式

下面结合附图对本发明做更进一步的解释。

在Windows操作系统下选用VC++6.0作为编程工具，对三维测量设备获取的多视角点云数据进行配准处理。该实例采用斯坦福大学的bunny点云数据，最终得到比较精确的配准结果。

如图1所示，三维点云后续处理中需要对不同视角的点云进行配准，针对现有的基于特征的自动配准算法中特征辨识度低、对噪声敏感以及对点云拓扑结构要求高等问题，本发明提出了一种基于法向量信息的点云自动配准算法。首先根据区域点云法向量的变化程度选取用于配准的特征点集，然后对特征点集中每个点建立一个直方图统计特征，基于此直方图特征建立匹配点对，结合刚性距离约束条件并应用RANSAC(randomsampleconsensus)算法得到精确匹配点对，采用四元素法计算初始配准参数，最后用改进的ICP(iterativeclosestpoint)算法对点云精确配准。

具体实施步骤如下：

步骤1：基于法向量信息获取待匹配两个点云中的特征点集,具体步骤如下：

步骤1.1：采用三维扫描仪获取带有法向量信息的多视角点云数据,且相邻视角获得的点云间有重叠部分，定义点云中重叠部分的某一点p_i的特征度f_i为点p_i的法向量与其k个近邻点法向量夹角的算术平均值：

$f_i=\frac{1}{k}{\mathrm{\&Sigma;}}_{j=1}^k{\mathrm{\&theta;}}_{\mathrm{ij}}$ （1）式

其中某一点p_i的k个近邻点是指与点p_i欧氏距离最近的k个点，θ_ij为点p_i的法向量与其近邻点p_j的法向量的夹角,k为5≤k≤20的自然数；

步骤1.2：选取阈值ε₁，去掉点云中f_i≤ε₁的平坦部分，保留f_i＞ε₁的点，对于保留点中的任一点p_m，若其满足：

f(p_m)＝max(f(p_m1),f(p_m2),…,f(p_mk))（2）式

则将p_m作为特征点,其中f(p_m1),f(p_m2),…,f(p_mk)为点p_m的k个近邻点的特征度，其中阈值ε₁取值范围为5°≤ε₁≤10°；

步骤1.3：利用步骤1.1和步骤1.2所述的特征点提取方法，分别对两个点云进行特征点提取，设待配准的两个点云数据分别为点集P和点集Q，其中点集Q为参考点云数据，得到待配准点云的特征点集为Pt＝{pt₁,pt₂,pt₃,…pt_m′}，参考点云的特征点集为Qt＝{qt₁,qt₂,qt₃,…qt_n′}，其中m′和n′分别为P和Q的特征点的个数；

如图3所示，从左至右依次为当k＝10,ε₁＝5°、k＝10,ε₁＝8°、k＝16,ε₁＝5°、k＝16,ε₁＝8°时的特征点集图。

步骤2：建立特征点集的直方图特征描述，方法如下：

步骤2.1：对于特征点集Pt中的每个点pt_i，在点集P中以pt_i为原点，半径为γ的球域内的点作为pt_i的邻近点，标记为N(pt_i)，其中γ取值范围为点云的平均点间距离的5～10倍；

步骤2.2：根据点pt_i与邻近点N(pt_i)之间的几何关系，建立三种特征描述：

f₁＝acos<n_i,v_k>（3）式

f₂＝<n_i,(s_k-pt_i)>（4）式

f₃＝‖s_k-pt_i‖（5）式

其中n_i为点pt_i的法向量，v_k为pt_i邻近某个点N(pt_i)的法向量，s_k为pt_i某个邻近点N(pt_i)的三维坐标；

其中，式（3）中特征值f₁是点集Pt中一点pt_i的法向量与其邻近一点N(pt_i)的法向量的夹角，由于这一特征在多数情况下主要分布在0～60度间，根据夹角大小将其分成[0,20]、(20,40]、(40,60]和(60,180]4个间隔；式（4）中特征值f₂是两个向量点积，其中一个是点pt_i的法向量，另一个是该点pt_i与其邻近一点N(pt_i)之间的点间向量，根据f₂是否大于0，将f₂分成2个间隔；式（5）中特征值f₃是一点pt_i与其邻近点中一点N(pt_i)间的欧氏距离，根据阈值将其分成2个间隔；根据这三个特征值的间隔分类，我们可建立一个间隔数为4×2×2＝16的直方图，对应地得到一个16维的特征向量，其中γ取值范围为点云的平均点间距离的5～10倍；

步骤2.3：根据所述步骤2.2中三个特征值f₁、f₂、f₃，定义idex＝k₁+k₂·4+k₃·4·2；

若特征值f₁落入[0,20]间隔中则对应地将k₁记为1，若特征值f₁落入(20,40]间隔中则对应地将k₁记为2，若特征值f₁落入(40,60]间隔中则对应地将k₁记为3，若特征值f₁落入(60,180]间隔中则对应地将k₁记为4；若特征值f₂的值小于0，则将k₂记为1，否则记为0；若特征值f₃的值大于则将k₃记为1，否则记为0；

根据idex的值确定N(pt_i)中一点属于直方图中的间隔位置，遍历一个点pt_i的所有邻近点N(pt_i)，得到落入每个间隔中的邻近点N(pt_i)的数量，以N(pt_i)中落入每个间隔中的点数占其总数的百分比作为对应间隔的值，记此值为特征向量h1_i；

如图4所示，点云bunny45上一个特征点的直方图特征图。

步骤2.4：根据所述步骤2.1至2.3，建立点集Pt和Qt中每个点的特征向量，得到点集Pt和Qt的特征向量集h1和h2；

步骤3：采取特征向量间的欧氏距离作为比较准则，比较所述步骤2中获得的特征向量集h1和h2中的特征向量，在点集Qt中为点集Pt中每个点找到其初始匹配点：选取阈值ε₂，特征向量的距离小于ε₂时，该特征向量所对应点集Pt和Qt中的点作为初始匹配点对，记为：

$\mathrm{Matchdots}=\left\{(s_i^1,s_i^2)\right|s_i^1\&Element;\mathrm{Pt},s_i^2\&Element;\mathrm{Qt},i=\mathrm{1,2,3}\&CenterDot;\&CenterDot;\&CenterDot;\mathrm{num}\left(\mathrm{Matchdots}\right)\}$

其中num(Matchdots)是初始匹配点对的数量，ε₂取0.05。

如图5所示为点云bunny的两个视角的初始配准结果。

步骤4：结合刚性距离约束条件，使用一种自适应的RANSAC算法获取精确匹配点对，具体步骤如下：

步骤4.1：Matchdots中的任意两个点对和如果是正确的匹配点对，那么根据刚体变换的距离不变性得到：其中dist表示两点间的距离；选取ε₃＞0，对每个点对计算Matchdots中除点对以外与该点对符合刚性距离约束的点对的数目num_i，若Matchdots中一个点对满足下式：

$\frac{|\mathrm{dist}(s_i^1,s_j^1)-\mathrm{dist}(s_i^2,s_j^2)|}{\mathrm{dist}(s_i^1,s_j^1)+\mathrm{dist}(s_i^2,s_j^2)}<{\mathrm{\&epsiv;}}_3$ （6）式

则将点对记为相对于点对的符合距离约束的点对，同时将的num_i值加1，其中ε₃取0.02；

步骤4.2：对Matchdots中每个点对根据所述步骤4.1计算num_i值，然后按照num_i值从大到小对Matchdots中点对进行排序，选取前N个点对，N为N≤num(Matchdots)的自然数；使用RANSAC算法对这N个点对检验是否为正确匹配点对，具体检验方法为：

步骤4.2.1：从这N个点对中随机选取3个点对作为一个样本；

步骤4.2.2：假设这3个点对为正确匹配点对，计算刚体变换矩阵T；

步骤4.2.3：判断其余N-3个点对在此刚体变换矩阵T下是否为正确匹配：

如果小于预定义的阈值ε₄，则认为是正确的匹配点对，记为内点；如果大于等于预定义的阈值ε₄，则认为是错误的匹配点对，记为外点；所有内点组成本次采样的一致集；其中，ε₄取值范围为点云的平均点间距离的2倍；

步骤4.2.4：重复所述步骤4.2.1至步骤4.2.3，直到N个点中所有3点组合的样本都遍历了，循环次数达到采样次数上限后，将内点数目最多的刚体变换矩阵作为正确的刚体变换矩阵，据此变换矩阵求得初始匹配点对中的内点作为最终的精确匹配点对，记为：

Matchdots1＝{(m_i,m′_i)|m_i∈P,m′_i∈Q,i＝1,2,3…n}

其中n为精确匹配点对的数目；

步骤5：利用四元素法计算初始配准参数，具体步骤如下：

步骤5.1：计算点集{m_i|m_i∈P，i＝1,2,...,n}的质心μ和{m′_i|m′_i∈Q，i＝1,2,...,n}的质心μ′：

$\left\{\begin{array}{c}\mathrm{\&mu;}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{m}_i\\ {\mathrm{\&mu;}}^{\&prime;}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{m^{\&prime;}}_i\end{array}\right.$ （7）式

式（7）中，n为精确匹配点对的数目；

步骤5.2：将点集{m_i|m_i∈P，i＝1,2,...,n}和{m′_i|m′_i∈Q，i＝1,2,...,n}做相对于质心μ、μ′的平移，设平移后的点集分别为{h_i}和{h′_i}：

$\left\{\begin{array}{c}{h}_i=m_i-\mathrm{\&mu;}\\ {h^{\&prime;}}_i={m^{\&prime;}}_i-{\mathrm{\&mu;}}^{\&prime;}\end{array}\right.$ （8）式

步骤5.3：根据平移后的所得点集{h_i}和{h′_i}计算相关矩阵K：

$K=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{h}_i{\left({h^{\&prime;}}_i\right)}^T$ （9）式

式（9）中，n为精确匹配点对的数目；

步骤5.4：由相关矩阵K中各元素K_ij(i,j＝1,2,3,4)构造出四维对称矩阵K_a：

$K_a=\left[\begin{array}{cccc}{K}_{11}+K_{22}+K_{33}& K_{32}-K_{23}& K_{13}-K_{31}& K_{21}-K_{12}\\ K_{32}-K_{23}& K_{11}-K_{22}-K_{33}& K_{12}+K_{21}& K_{31}+K_{13}\\ K_{13}-K_{31}& K_{12}+K_{21}& {-K}_{11}+K_{22}-K_{33}& K_{23}+K_{32}\\ K_{21}-K_{12}& K_{31}+K_{13}& K_{23}+K_{32}& {-K}_{11}-K_{22}+K_{33}\end{array}\right]$ （10）式

步骤5.5：计算K_a的最大特征根所对应的单位特征向量q：

$q={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$ （11）式

步骤5.6：由步骤5.5中计算所得单位特征向量q，计算旋转矩阵R：

$R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2+q_2^2-q_1^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2+q_3^2-q_1^2-q_2^2\end{array}\right]$ （12）式

步骤5.7：由步骤5.6计算所得旋转矩阵R，求解平移矢量T：

T＝μ′-Rμ（13）式

如图5所示为bunny两个视角点云图初始配准后得到的结果图，从图中可以看出本方法初始配准即可得到较为满意的结果。

步骤6：采用改进的ICP算法对点云精确配准，具体步骤如下：

步骤6.1：利用步骤5中获得的配准参数R和T，根据式（14）将待配准点云P中的每个点p_i变换到参考点云Q所在的坐标系下得到点p_0i，将转换后得到的初始配准后的点云记为P₀；其中：

p_0i＝R·p_i+T（14）式

步骤6.2：令目标点集为P_m,参考点集为Q,取m＝0,利用改进的ICP算法进行精确配准：

步骤6.2.1：计算最近点：

对目标点集P_m中任一点p_mi,计算该点与参考点集Q中每个点间的欧氏距离,对于点集Q中与其欧氏距离最小的点q_j，选取阈值σ,若：两者间欧氏距离||p_mi,q_j||²＜σ，则将p_mi作为内点并记为并将q_j作为其对应匹配点并记为这样便得到一组内点匹配点对否则将其作为外点去除；其中当m＝0时,阈值σ取为点云Q点间平均距离的3倍；

对目标点集P_m中所有点作以上操作，最终得到内点匹配点集，记为： $M^m=\left\{(p_i^m,q_i^m)\right|p_i^m\&Element;P_m,q_i^m\&Element;Q,i=\mathrm{1,2,3}\&CenterDot;\&CenterDot;\&CenterDot;n_p\},$ 其中n_p为点对数目；

步骤6.2.2：利用步骤6.2.1中获得内点匹配点集来计算配准参数：

计算使得式（15）中err取得最小时的旋转矩阵R_m和平移矩阵T_m作为新的配准参数：

$\mathrm{err}=\frac{1}{n_p}\underset{i=1}{\overset{n_p}{\mathrm{\&Sigma;}}}{\left|\right|q_i^m-(R_m{p}_i^m+T_m)\left|\right|}^2$ （15）式

其中err为本次配准的误差，n_p为点对数目；

步骤6.2.3：将计算得到的配准参数用于点集P_m中的每个点p_mi得到新的点集P′

$p_i^{\&prime;}=R_m{p}_{\mathrm{mi}}+T_m$ （16）式

其中，为点集P′中的点；

步骤6.2.4：检验迭代终止的条件：

设定阈值τ=0.00001，当满足：σ-err＜τ，则迭代过程结束；否则令m＝m+1，σ＝err,P_m＝P′转步骤6.2.1；

迭代过程结束后得到的点集P′即为最终的精确配准结果。

如图6所示为bunny两个视角点云图精确配准后得到的结果图，从图中可以看出本发明方法的有效性，配准的效果较好。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (1)

1.一种基于法向量的点云自动配准方法，其特征在于：包括以下步骤：

步骤1：基于法向量信息获取待匹配两个点云中的特征点集,具体步骤如下：

步骤1.1：采用三维扫描仪获取带有法向量信息的多视角点云数据,且相邻视角获得的点云间有重叠部分，定义点云中重叠部分的某一点p_i的特征度w_i为点p_i的法向量与其k个近邻点法向量夹角的算术平均值：

$w_i=\frac{1}{k}{\mathrm{\&Sigma;}}_{j=1}^k{\mathrm{\&theta;}}_{ij}---\left(1\right)$

其中某一点p_i的k个近邻点是指与点p_i欧氏距离最近的k个点，θ_ij为点p_i的法向量与其近邻点p_j的法向量的夹角,k为5≤k≤20的自然数；

步骤1.2：选取阈值ε₁，去掉点云中w_i≤ε₁的平坦部分，保留w_i>ε₁的点，对于保留点中的任一点p_m，若其满足：

w(p_m)＝max(w(p_m1),w(p_m2),…,w(p_mk))(2)

则将p_m作为特征点,其中w(p_m1),w(p_m2),…,w(p_mk)为点p_m的k个近邻点的特征度，其中阈值ε₁取值范围为5°≤ε₁≤10°；

步骤1.3：利用步骤1.1和步骤1.2的特征点提取方法，分别对两个点云进行特征点提取，设待配准的两个点云数据分别为点集P和点集Q，其中点集Q为参考点云数据，得到待配准点云的特征点集为Pt＝{pt₁,pt₂,pt₃,…,pt_m'}，参考点云的特征点集为Qt＝{qt₁,qt₂,qt₃,…,qt_n'}，其中m'和n'分别为P和Q的特征点的个数；

步骤2：建立特征点集的直方图特征描述，方法如下：

步骤2.1：对于特征点集Pt中的每个点pt_i，在点集P中以pt_i为原点，半径为γ的球域内的点作为pt_i的邻近点，标记为N(pt_i)，其中γ取值范围为点云的平均点间距离的5～10倍；

步骤2.2：根据点pt_i与邻近点N(pt_i)之间的几何关系，建立三种特征描述：

f₁＝acos<n_i,v_k>(3)

f₂＝<n_i,(s_k-pt_i)>(4)

f₃＝||s_k-pt_i||(5)

其中n_i为点pt_i的法向量，v_k为pt_i邻近某个点N(pt_i)的法向量，s_k为pt_i某个邻近点N(pt_i)的三维坐标；

其中，式(3)中特征值f₁是点集Pt中一点pt_i的法向量与其邻近一点N(pt_i)的法向量的夹角，根据夹角大小将其分成[0,20]、(20,40]、(40,60]和(60,180]4个间隔；式(4)中特征值f₂是两个向量点积，其中一个是点pt_i的法向量，另一个是该点pt_i与其邻近一点N(pt_i)之间的点间向量，根据f₂是否大于0，将f₂分成2个间隔；式(5)中特征值f₃是一点pt_i与其邻近点中一点N(pt_i)间的欧氏距离，根据阈值将其分成2个间隔；根据这三个特征值的间隔分类，我们可建立一个间隔数为4×2×2＝16的直方图，对应地得到一个16维的特征向量，其中γ取值范围为点云的平均点间距离的5～10倍；

步骤2.3：根据所述步骤2.2中三个特征值f₁、f₂、f₃，定义idex＝k₁+k₂·4+k₃·4·2；

若特征值f₁落入[0,20]间隔中则对应地将k₁记为1，若特征值f₁落入(20,40]间隔中则对应地将k₁记为2，若特征值f₁落入(40,60]间隔中则对应地将k₁记为3，若特征值f₁落入(60,180]间隔中则对应地将k₁记为4；若特征值f₂的值小于0，则将k₂记为1，否则记为0；若特征值f₃的值大于则将k₃记为1，否则记为0；

根据idex的值确定N(pt_i)中一点属于直方图中的间隔位置，遍历一个点pt_i的所有邻近点N(pt_i)，得到落入每个间隔中的邻近点N(pt_i)的数量，以N(pt_i)中落入每个间隔中的点数占其总数的百分比作为对应间隔的值，记此值为特征向量h1_i；

步骤2.4：根据所述步骤2.1至2.3，建立点集Pt和Qt中每个点的特征向量，得到点集Pt和Qt的特征向量集h1和h2；

步骤3：采取特征向量间的欧氏距离作为比较准则，比较所述步骤2中获得的特征向量集h1和h2中的特征向量，在点集Qt中为点集Pt中每个点找到其初始匹配点：选取阈值ε₂，特征向量的距离小于ε₂时，该特征向量所对应点集Pt和Qt中的点作为初始匹配点对，记为：

$Matchdots=\left\{(s_i^1,s_i^2)\right|s_i^1\&Element;Pt,s_i^2\&Element;Qt,i=1,2,3...num\left(Matchdots\right)\}$

其中num(Matchdots)是初始匹配点对的数量，ε₂取0.05；

步骤4：结合刚性距离约束条件，使用一种自适应的RANSAC算法获取精确匹配点对，具体步骤如下：

步骤4.1：Matchdots中的任意两个点对和如果是正确的匹配点对，那么根据刚体变换的距离不变性得到：其中dist表示两点间的距离；选取ε₃>0，对每个点对计算Matchdots中除点对以外与该点对符合刚性距离约束的点对的数目num_i，若Matchdots中一个点对满足下式：

$\frac{|dist(s_i^1,s_j^1)-dist(s_i^2,s_j^2)|}{dist(s_i^1,s_j^1)+dist(s_i^2,s_j^2)}<{\mathrm{\&epsiv;}}_3---\left(6\right)$

则将点对记为相对于点对的符合距离约束的点对，同时将的num_i值加1，其中ε₃取0.02；

步骤4.2：对Matchdots中每个点对根据所述步骤4.1计算num_i值，然后按照num_i值从大到小对Matchdots中点对进行排序，选取前N个点对，N为N≤num(Matchdots)的自然数；使用RANSAC算法对这N个点对检验是否为正确匹配点对，具体检验方法为：

步骤4.2.1：从这N个点对中随机选取3个点对作为一个样本；

步骤4.2.2：假设这3个点对为正确匹配点对，计算刚体变换矩阵T；

步骤4.2.3：判断其余N-3个点对在此刚体变换矩阵T下是否为正确匹配：

如果小于预定义的阈值ε₄，则认为是正确的匹配点对，记为内点；如果大于等于预定义的阈值ε₄，则认为是错误的匹配点对，记为外点；所有内点组成本次采样的一致集；其中，ε₄取值范围为点云的平均点间距离的2倍；

步骤4.2.4：重复所述步骤4.2.1至步骤4.2.3，直到N个点中所有3点组合的样本都遍历了，循环次数达到采样次数上限后，将内点数目最多的刚体变换矩阵作为正确的刚体变换矩阵，据此变换矩阵求得初始匹配点对中的内点作为最终的精确匹配点对，记为：

Matchdots1＝{(m_i,m'_i)|m_i∈P,m'_i∈Q,i＝1,2,3,…,n}

其中n为精确匹配点对的数目；

步骤5：利用四元素法计算初始配准参数，具体步骤如下：

步骤5.1：计算点集{m_i|m_i∈P，i＝1,2,...,n}的质心μ和{m'_i|m'_i∈Q，i＝1,2,...,n}的质心μ'：

$\{\begin{array}{c}\mathrm{\&mu;}=\frac{1}{n}\underset{i=1}{\overset{n}{\&Sigma;}}{m}_i\\ {\mathrm{\&mu;}}^{\&prime;}=\frac{1}{n}\underset{i=1}{\overset{n}{\&Sigma;}}{m^{\&prime;}}_i\end{array}---\left(7\right)$

式(7)中，n为精确匹配点对的数目；

步骤5.2：将点集{m_i|m_i∈P，i＝1,2,...,n}和{m'_i|m'_i∈Q，i＝1,2,...,n}做相对于质心μ、μ'的平移，设平移后的点集分别为{h_i}和{h'_i}：

$\left\{\begin{array}{c}{h}_i=m_i-\mathrm{\&mu;}\\ {h^{\&prime;}}_i={m^{\&prime;}}_i-{\mathrm{\&mu;}}^{\&prime;}\end{array}\right.---\left(8\right)$

步骤5.3：根据平移后的所得点集{h_i}和{h'_i}计算相关矩阵K：

$K=\frac{1}{n}\underset{i=1}{\overset{n}{\&Sigma;}}{h}_i{\left({h^{\&prime;}}_i\right)}^T---\left(9\right)$

式(9)中，n为精确匹配点对的数目；

步骤5.4：由相关矩阵K中各元素K_ij(i,j＝1,2,3)构造出四维对称矩阵K_a：

$K_a=\left[\begin{array}{cccc}{K}_{11}+K_{22}+K_{33}& K_{32}-K_{23}& K_{13}-K_{31}& K_{21}-K_{12}\\ K_{32}-K_{23}& K_{11}-K_{22}-K_{33}& K_{12}+K_{21}& K_{31}+K_{13}\\ K_{13}-K_{31}& K_{12}+K_{21}& -K_{11}+K_{22}-K_{33}& K_{23}+K_{32}\\ K_{21}-K_{12}& K_{31}+K_{13}& K_{23}+K_{32}& -K_{11}-K_{22}+K_{33}\end{array}\right]---\left(10\right)$

步骤5.5：计算K_a的最大特征根所对应的单位特征向量q：

q＝[q₀q₁q₂q₃]^T(11)

步骤5.6：由步骤5.5中计算所得单位特征向量q，计算旋转矩阵R：

$R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2+q_2^2-q_1^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2+q_3^2-q_1^2-q_2^2\end{array}\right]---\left(12\right)$

步骤5.7：由步骤5.6计算所得旋转矩阵R，求解平移矢量T：

T＝μ'-Rμ(13)

步骤6：采用改进的ICP算法对点云精确配准，具体步骤如下：

步骤6.1：利用步骤5中获得的配准参数R和T，根据式(14)将待配准点云P中的每个点p_i变换到参考点云Q所在的坐标系下得到点p_0i，将转换后得到的初始配准后的点云记为P₀；其中：

p_0i＝R·p_i+T(14)

步骤6.2：令目标点集为P_m,参考点集为Q,取m＝0,利用改进的ICP算法进行精确配准：

步骤6.2.1：计算最近点：

对目标点集P_m中任一点p_mi,计算该点与参考点集Q中每个点间的欧氏距离,对于点集Q中与其欧氏距离最小的点q_j，选取阈值σ,若：两者间欧氏距离||p_mi,q_j||²<σ，则将p_mi作为内点并记为并将q_j作为其对应匹配点并记为这样便得到一组内点匹配点对否则将其作为外点去除；其中当m＝0时,阈值σ取为点云Q点间平均距离的3倍；

对目标点集P_m中所有点作以上操作，最终得到内点匹配点集，记为： $M^m=\left\{(p_i^m,q_i^m)\right|p_i^m\&Element;P_m,q_i^m\&Element;Q,i=1,2,3...n_p\},$ 其中n_p为点对数目；

步骤6.2.2：利用步骤6.2.1中获得内点匹配点集来计算配准参数：

计算使得式(15)中err取得最小时的旋转矩阵R_m和平移矩阵T_m作为新的配准参数：

$err=\frac{1}{n_p}\underset{i=1}{\overset{n_p}{\&Sigma;}}\left|\right|q_i^m-(R_m{p}_i^m+T_m)|{|}^2---\left(15\right)$

其中err为本次配准的误差，n_p为点对数目；

步骤6.2.3：将计算得到的配准参数用于点集P_m中的每个点p_mi得到新的点集P'

p′_i＝R_mp_mi+T_m(16)

其中，p′_i为点集P'中的点；

步骤6.2.4：检验迭代终止的条件：

设定阈值τ＝0.00001，当满足：σ-err<τ，则迭代过程结束；否则令m＝m+1，σ＝err,P_m＝P'转步骤6.2.1；

迭代过程结束后得到的点集P'即为最终的精确配准结果。