CN104200507A - 一种三维点云点法向量估计方法 - Google Patents

Abstract

本发明公开了一种三维点云点法向量估计方法，在空间邻域中确定各点的k近邻点，根据k近邻点计算各点的初始法向量，拟合平面，根据k近邻点到平面的距离远近选出距离最近60％～80％的k近邻点，在第一次选出的k近邻点中再选择与该点的法向量夹角最小的60％～80％的k近邻点，剔除噪声点，计算选出点的稳定的法向量，遍历点云，得到各点稳定的法向量。本发明先后使用距离判断和方向一致性判断剔除k近邻中不稳定的点，从而计算得到点云中点的较稳定的法向量，为点云的配准、聚类、分割等提供较稳定的计算参数，提高对噪声的抑制能力。

Description

一种三维点云点法向量估计方法

技术领域

本发明涉及一种参数计算方法，特别是一种三维点云点法向量估计方法。

背景技术

随着传感器技术的进步，三维点云的获取越来越容易，如何理解以点云表示的三维场景是智能机器人导航、环境建模、体感游戏等领域要解决的重要问题。单纯使用点的三维空间坐标进行计算和分析一个点云场景是难以实现的，使用几何推理技术提取紧凑、有效的点特征能够更好的描述点云，便于开展下一步的计算。一个点的法向量是使用的最广泛的特征，广泛应用在数据配准、分割等方面。

目前常用计算一个点与其近邻点的协方差矩阵特征值和特征向量的方法估计点云中点的法向量的方向，这种方法对噪声比较敏感，而点云数据本身经常含有大量的噪声，因此采用这种方法在估计点云中点的法向量时容易引入较大的误差。

发明内容

本发明的目的在于提供一种三维点云点法向量估计方法，能够较好的估计含有噪声数据的点云中点的法向量。

实现本发明目的的技术解决方案为：一种三维点云点法向量估计方法，包括以下步骤：

步骤1、设一个三维点云的点集合为P，P中点的个数为N，取P中一点p_i(x_i,y_i,z_i)(p_i∈P∈R³)，1≤i≤N，在空间邻域中确定p_i的k近邻点，其中k≥30，根据k近邻点确定p_i的初始法向量；遍历点云P，确定P中每个点的初始法向量；

步骤2、对p_i拟合平面；

步骤3、根据p_i的k近邻点到拟合平面的距离远近选出距离最近的p个k近邻点，其中p为正整数且0.6k≤p≤0.8k；

步骤4、从步骤3选出的p个k近邻点中选择与p_i的初始法向量夹角最小的m个k近邻点，其中m为正整数且0.6p≤m≤0.8p；

步骤5、根据步骤4选出的m个k近邻点确定p_i的稳定的法向量；

步骤6、遍历点云P，确定P中每个点稳定的法向量。

本发明与现有技术相比，其显著优点为：(1)本发明在计算坐标协方差矩阵时，使用点集的中值代替均值，使结果更加稳定；(2)本发明根据点到平面的距离以及点法向量与拟合平面方向之间的夹角剔除k近邻中不稳定的点，提高了对噪声的抑制能力；(3)本发明的三维点云点法向量估计方法运用在光照计算中能明显改善三维模型的视觉显示效果。

下面结合附图对本发明做进一步说明。

附图说明

图1为本发明的三维点云点法向量估计方法的流程图。

图2为本发明实施例的初始法向量分布示意图。

图3为本发明实施例的稳定的法向量分布示意图。

具体实施方式

一种三维点云点法向量估计方法，包括以下步骤：

步骤1、设一个三维点云的点集合为P，P中点的个数为N，取P中一点p_i(x_i,y_i,z_i)(p_i∈P∈R³)，1≤i≤N，其中x_i、y_i、z_i分别为点p_i的X坐标、Y坐标和Z坐标，在空间邻域中确定p_i的k近邻点，其中k≥30，根据k近邻点确定p_i的初始法向量；遍历点云P，确定P中每个点的初始法向量；

步骤1-1、设p_i的k近邻点集合为N_pi；

步骤1-2、定义p_i的坐标协方差矩阵为

C为一个3×3的对称半正定矩阵，计算C的3个特征值并按降序排列使λ₂≥λ₁≥λ₀，三个特征值分别对应特征向量v₂，v₁，v₀；取最小特征值对应的特征向量v₀作为点p_i的初始法向量n_i；

若最小特征值λ₀对应的特征向量有两个解，设2个解分别为n_i1和n_i2，从点p_i指向坐标原点的向量为分别计算n_i1和n_i2与的夹角，即

取夹角不大于90°的特征向量作为p_i的初始法向量n_i，如果两个夹角都刚好等于90°，则在n_i1和n_i2中任取1个作为p_i的初始法向量n_i；

步骤2、对p_i拟合平面；

设p_i的k近邻点为p_ij(x_ij,y_ij,z_ij)，j＝1,2,...,k，采用最小二乘法进行拟合平面，拟合平面方程为Ax+By+z+D＝0，则

$\left\{\begin{array}{c}A=-\left(z_1{b}_2{-z}_2{b}_2\right)/(a_1{b}_2-a_2{b}_1)\\ B=-(a_1{z}_2-a_2{z}_1)/(a_1{b}_2-a_2{b}_1)\\ D=-(\mathrm{\Σz}+\mathrm{A\Σx}+\mathrm{B\Σy})/k\end{array}\right.---\left(3\right)$

其中：

$\begin{array}{c}{a}_1=\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}^2-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}},b_1=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{y}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},z_1=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}\\ a_2=\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{y}_{\mathrm{ij}}--\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},b_2=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}^2-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},z_2=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}{z}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}\end{array}---\left(4\right)$

步骤3、根据p_i的k近邻点到平面的距离远近选出距离最近的p个k近邻点，其中p为正整数且0.6k≤p≤0.8k；具体包括以下步骤：

步骤3-1、设平面的单位法向量为n_plan-i，中心点为分别计算k近邻点到该平面的距离

$\mathrm{OD}\left(p_{\mathrm{ij}}\right)={(p_{\mathrm{ij}}-{\stackrel{\‾}{p}}_{\mathrm{plan}-i})}^T\·n_{\mathrm{plan}-i},j=\mathrm{1,2},...,k---\left(5\right)$

步骤3-2、将计算得到的k个距离按照从小到大排序，取到拟合平面距离较小的p个k近邻点形成点集P_iOD，其中p为正整数且0.6k≤p≤0.8k；

步骤4、从步骤3选出的p个k近邻点中选择与p_i的初始法向量夹角最小的m个k近邻点，其中m为正整数且0.6p≤m≤0.8p；具体包括以下步骤：

步骤4-1、设P_iOD中一点的初始法向量为n_iOD，n_iOD与点p_i的初始法向量n_i的夹角为遍历P_iOD，确定P_iOD中所有点的初始法向量与点p_i的初始法向量n_i的夹角；

步骤4-2、将夹角按照从小到大排序，再取夹角较小的m个k近邻点形成点集P_iODAng，其中m为正整数且0.6p≤m≤0.8p；

步骤4-3、把不包含在P_iODAng中的点p_i的k近邻点作为噪声点剔除。

步骤5、根据步骤4选出的m个k近邻点计算p_i的稳定的法向量；具体包括以下步骤：

步骤5-1、使用点集P_iODAng中的点计算点p_i的稳定的坐标协方差矩阵

步骤5-2、计算C_R的3个特征值并按降序排列使λ_R2≥λ_R1≥λ_R0，三个特征值分别对应特征向量v_R2，v_R1，v_R0；

步骤5-3、最小特征值λ_R0对应的特征向量v_R0为点p_i的稳定的法向量n_Ri，当v_R0有2个解时，采用步骤1-2中计算初始法向量中的方法取与的夹角不大于90°的特征向量作为点p_i的法向量。

步骤6、遍历点云P，根据步骤2-步骤5计算P中每个点稳定的法向量。

下面结合具体实施例对本发明做进一步说明。

实施例1

结合图1，使用本发明的三维点云点法向量估计方法计算平面上点的稳定的法向量，包括以下步骤：

步骤1、设一个三维点云的点集合为P，P中点的个数为600，取P中一点p_i(x_i,y_i,z_i)(p_i∈P∈R³)，1≤i≤600，在空间邻域中确定p_i的k近邻点，其中k＝30，根据k近邻点确定p_i的初始法向量；遍历点云P，确定P中每个点的初始法向量；具体包括以下步骤：

步骤1-1、设p_i的k近邻点集合为N_pi；

步骤1-2、定义p_i的坐标协方差矩阵为

C为一个3×3的对称半正定矩阵，计算C的3个特征值并按降序排列使λ₂≥λ₁≥λ₀，三个特征值分别对应特征向量v₂，v₁，v₀；那么最小特征值对应的特征向量v₀就是点p_i的初始法向量n_i；

若最小特征值λ₀对应的特征向量有两个解，需要选1个解作为点p_i的初始法向量，设2个解分别为n_i1和n_i2，从点p_i指向坐标原点的向量为那么分别计算n_i1和n_i2与的夹角，即

取夹角不大于90°的作为p_i的初始法向量n_i，如果两个夹角均为90°，则在n_i1和n_i2中任取1个作为p_i的初始法向量n_i；

步骤2、对p_i使用最小二乘法拟合平面；

设p_i的k近邻点为p_ij(x_ij,y_ij,z_ij)，j＝1,2,...,30，待拟合平面方程为Ax+By+z+D＝0，则

$\left\{\begin{array}{c}A=-\left(z_1{b}_2{-z}_2{b}_2\right)/(a_1{b}_2-a_2{b}_1)\\ B=-(a_1{z}_2-a_2{z}_1)/(a_1{b}_2-a_2{b}_1)\\ D=-(\mathrm{\Σz}+\mathrm{A\Σx}+\mathrm{B\Σy})/k\end{array}\right.---\left(3\right)$

其中：

$\begin{array}{c}{a}_1=\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}^2-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}},b_1=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{y}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},z_1=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}\\ a_2=\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{y}_{\mathrm{ij}}--\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},b_2=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}^2-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},z_2=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}{z}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}\end{array}---\left(4\right)$

步骤3、根据p_i的k近邻点到拟合平面的距离远近选出距离最近的80％的k近邻点，即24个k近邻点；具体包括以下步骤：

步骤3-1、设平面的单位法向量为n_plan-i，中心点为分别计算k近邻点到该平面的距离

$\mathrm{OD}\left(p_{\mathrm{ij}}\right)={(p_{\mathrm{ij}}-{\stackrel{\‾}{p}}_{\mathrm{plan}-i})}^T\·n_{\mathrm{plan}-i},j=\mathrm{1,2},...,30---\left(5\right)$

步骤3-2、将计算得到的30个距离按照从小到大排序，取到拟合平面距离较小的24个k近邻点形成点集P_iOD；

步骤4、在步骤3选出的24个k近邻点中选择与该点的初始法向量夹角最小的80％的k近邻点；具体包括以下步骤：

步骤4-1、用公式(2)计算两个向量夹角的方法计算P_iOD中的点的初始法向量与点p_i的初始法向量n_i的夹角；

步骤4-2、将夹角按照从小到大排序，再取夹角较小的80％的点形成点集P_iODAng，P_iODAng中点的个数为19个；

步骤4-3、把不包含在P_iODAng中的点p_i的k近邻点作为噪声点剔除；

步骤5、使用步骤4选出的k近邻点计算p_i的稳定的法向量；具体包括以下步骤：

步骤5-1、使用点集P_iODAng中的点计算点p_i的稳定的坐标协方差矩阵

步骤5-2、计算C_R的3个特征值并按降序排列使λ_R2≥λ_R1≥λ_R0，三个特征值分别对应特征向量v_R2，v_R1，v_R0；

步骤5-3、最小特征值λ_R0对应的特征向量v_R0为点p_i的稳定的法向量n_Ri，当v_R0有2个解时，采用步骤1-2中计算初始法向量中的方法取与的夹角不大于90°的特征向量作为点p_i的法向量。

步骤6、遍历点云P，根据步骤2-步骤5计算P中每个点的稳定的法向量。

结合图2，箭头为点的初始法向量分布情况，为了显示的清楚，将3D点云并旋转到侧视的位置，随机显示约5％的点的法向量，可以看到其初始法向量分布有些杂乱。

结合图3，使用三维点云点法向量估计方法计算的稳定的法向量分布情况，随机显示约5％的点的法向量，且剔除了算法中计算得到的公共的噪声点，可以看到稳定的法向量分布规整，符合位于平面上的采样点的法向量分布特点。

本发明在计算坐标协方差矩阵时，使用点集的中值代替均值，使结果更加稳定；使用距离判断和方向一致性判断剔除k近邻中不稳定的点，从而计算得到三维点云中点的较稳定的法向量，为三维点云的配准、聚类、分割等提供较稳定的计算参数，提高对噪声的抑制能力。

Claims (6)

1.一种三维点云点法向量估计方法，其特征在于，包括以下步骤：

步骤1、设一个三维点云的点集合为P，P中点的个数为N，取P中一点p_i(x_i,y_i,z_i)(p_i∈P∈R³)，1≤i≤N，在空间邻域中确定p_i的k近邻点，其中k≥30，根据k近邻点确定p_i的初始法向量；遍历点云P，确定P中每个点的初始法向量；

步骤2、对p_i拟合平面；

步骤3、根据p_i的k近邻点到拟合平面的距离远近选出距离最近的p个k近邻点，其中p为正整数且0.6k≤p≤0.8k；

步骤4、从步骤3选出的p个k近邻点中选择与p_i的初始法向量夹角最小的m个k近邻点，其中m为正整数且0.6p≤m≤0.8p；

步骤5、根据步骤4选出的m个k近邻点确定p_i的稳定的法向量；

步骤6、遍历点云P，确定P中每个点的稳定的法向量。

2.根据权利要求1所述的三维点云点法向量估计方法，其特征在于，步骤1中根据k近邻点确定p_i的初始法向量具体包括以下步骤：

步骤1-1、设p_i的k近邻点集合为N_pi；

步骤1-2、定义p_i的坐标协方差矩阵为

C为一个3×3的对称半正定矩阵，计算C的3个特征值并按降序排列使λ₂≥λ₁≥λ₀，三个特征值分别对应特征向量v₂、v₁和v₀；取最小特征值对应的特征向量v₀作为点p_i的初始法向量n_i；

若最小特征值λ₀对应的特征向量有两个解，设2个解分别为n_i1和n_i2，从点p_i指向坐标原点的向量为分别计算n_i1和n_i2与的夹角，即

取夹角不大于90°的特征向量作为p_i的初始法向量n_i，如果两个夹角都刚好等于90°，则n_i1和n_i2中任取1个作为p_i的初始法向量n_i。

3.根据权利要求1或2所述的三维点云点法向量估计方法，其特征在于，步骤2中对p_i拟合平面具体包括以下步骤：

设p_i的k近邻点为p_ij(x_ij,y_ij,z_ij)，j＝1,2,...,k，采用最小二乘法进行拟合平面，拟合平面方程为Ax+By+z+D＝0，则

$\left\{\begin{array}{c}A=-\left(z_1{b}_2{-z}_2{b}_2\right)/(a_1{b}_2-a_2{b}_1)\\ B=-(a_1{z}_2-a_2{z}_1)/(a_1{b}_2-a_2{b}_1)\\ D=-(\mathrm{\Σz}+\mathrm{A\Σx}+\mathrm{B\Σy})/k\end{array}\right.$

其中：

$\begin{array}{c}{a}_1=\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}^2-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}},b_1=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{y}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},z_1=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}\\ a_2=\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{y}_{\mathrm{ij}}--\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},b_2=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}^2-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}},z_2=k\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}{z}_{\mathrm{ij}}-\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{z}_{\mathrm{ij}}\end{array}.$

4.根据权利要求3所述的三维点云点法向量估计方法，其特征在于，步骤3中根据p_i的k近邻点到拟合平面的距离远近选出距离最近的p个k近邻点，其中p为正整数且0.6k≤p≤0.8k，具体包括以下步骤：

步骤3-1、设拟合平面的单位法向量为n_plan-i，中心点为分别计算k近邻点到该平面的距离

$\mathrm{OD}\left(p_{\mathrm{ij}}\right)={(p_{\mathrm{ij}}-{\stackrel{\‾}{p}}_{\mathrm{plan}-i})}^T\·n_{\mathrm{plan}-i},j=\mathrm{1,2},...,k$

步骤3-2、将得到的k个距离按照从小到大排序，取到拟合平面距离较小的p个k近邻点形成点集P_iOD，其中p为正整数且0.6k≤p≤0.8k。

5.根据权利要求4所述的三维点云点法向量估计方法，其特征在于，步骤4中所述从步骤3选出的p个k近邻点中选择与p_i的初始法向量夹角最小的m个k近邻点，其中m为正整数且0.6p≤m≤0.8p；具体包括以下步骤：

步骤4-1、设P_iOD中一点的初始法向量为n_iOD，n_iOD与点p_i的初始法向量n_i的夹角为遍历P_iOD，确定P_iOD中所有点的初始法向量与点p_i的初始法向量n_i的夹角；

步骤4-2、将夹角按照从小到大排序，再取夹角较小的m个k近邻点形成点集P_iODAng，其中m为正整数且0.6p≤m≤0.8p；

步骤4-3、把不包含在P_iODAng中的点p_i的k近邻点作为噪声点剔除。

6.根据权利要求5所述的三维点云点法向量估计方法，其特征在于，根据步骤4选出的m个k近邻点确定p_i的稳定的法向量，具体包括以下步骤：

步骤5-1、使用点集P_iODAng中的点确定点p_i的稳定的坐标协方差矩阵

步骤5-2、计算C_R的3个特征值并按降序排列使λ_R2≥λ_R1≥λ_R0，三个特征值分别对应特征向量v_R2，v_R1，v_R0；

步骤5-3、最小特征值λ_R0对应的特征向量v_R0为点p_i的稳定的法向量n_Ri，当v_R0有2个解时，采用步骤1-2中计算初始法向量中的方法取与的夹角不大于90°的特征向量作为点p_i的法向量。