CN112365528A - 一种基于主成分分析的三维点云模型逐步求精快速配准方法 - Google Patents

Abstract

本发明公开了一种基于主成分分析的三维点云模型逐步求精快速配准方法。步骤S1：ICP将初始配准后的点云模型通过PCA方法进行点云精简和法向量估算；步骤S2：将点云选取对应点对；步骤S3：将对应点对进行初始配准后，得到初始的变换矩阵，判断误差是否小于初始配准阈值，若小于初始配准阈值，则进行组合迭代精配准，若大于等于初始配准阈值，则返回步骤S2重新选取对应点对；步骤S4：将进行组合迭代精配准的点对，判断误差是否小于精配准阈值，若大于等于精配准阈值，则返回步骤S2重新选取对应点对，若小于精配准阈值，则配准结束。本发明采用快速点特征直方图，在初始配准的基础上通过实验对比ICP和NICP组合给出快速并且精准的配准方案。

Description

一种基于主成分分析的三维点云模型逐步求精快速配准方法

技术领域

本发明属于配准领域；具体涉及一种基于主成分分析的三维点云模型逐步求精快速配准方法。

背景技术

配准技术在三维模型的拼接、场景动态变化检测和物体的识别都有着重要作用。当获取两个模型在全局坐标系下位姿不同时，以两者中任意一个模型作为标准，对另一个模型做刚体变换使其与标准间的重合度达到极大值，以上即为配准过程。在本发明中所处理模型是文件存储格式为PCD的点云数据。

迭代最近点(ICP)是目前比较常用的配准算法，该算法交替的在两组模型中选择对应点对迭代变换模型的位姿，直到与标准模型的距离误差达到极小值。

ICP的迭代收敛速度受对应点对的选取以及误差函数影响较大,该算法是点到点的对应点选取方式，即定义距离最近的两点作为一组对应点，这种方法过于简单，使错误配对点的比例增大以及对噪声点、异常值非常的敏感。近几年来，很多对ICP进行了改进，NICP改变了ICP的点到点的选取方式，通过最小化点到点的距离在法向量定义的局部表面上的投影来选取对应点对，该算法需要模型的数据包含法向量信息，对法向量进行了估算，其中基于主成分分析(PCA)的算法估算速度较快，其基本思想是假设点云模型的采样表面处处光滑，通过拟合点云局部邻域的平面得到法向量。GICP通过引入最小化误差的概率解释使得算法对错误点对以及噪声点的鲁棒性增强。ICP、NICP和GICP都对模型的初始位姿要求较高，只有两组模型的初始重合度较高时配准才能够取得全局最优值。

发明内容

本发明采用逐步求精的配准方案，根据模型的特征描述初始配准并对模型降采样加快配准速度，特征描述的选择决定了初始配准的精度，其优劣一般通过平移旋转不变性、抗密度干扰性、对噪声具有鲁棒性来判断，本发明采用快速点特征直方图FPFH，在初始配准的基础上通过实验对比ICP和NICP组合给出快速并且精准的配准方案。

本发明通过以下技术方案实现：

一种基于主成分分析的三维点云模型逐步求精快速配准方法，所述逐步求精快速配准方法包括以下步骤，

步骤S1：ICP将初始配准后的点云模型通过PCA方法进行点云精简和法向量估算；

步骤S2：将精简与估算后的点云选取对应点对；

步骤S3：将选取的对应点对进行初始配准后，通过降采样模型的FPFH特征得到初始的变换矩阵，判断误差是否小于初始配准阈值，若小于初始配准阈值，则进行组合迭代精配准，若大于等于初始配准阈值，则返回步骤S2重新选取对应点对；

步骤S4：将进行组合迭代精配准的点对，判断误差是否小于精配准阈值，若大于等于精配准阈值，则返回步骤S2重新选取对应点对，若小于精配准阈值，则配准结束。

进一步的，所述步骤S1的法向量估算为，在点云集合

中找到距离采样点x_i最近的K个相邻的点作为x_i的最近K邻域，记为Nbhd(x_i)，通过Nbhd(x_i) 拟合平面得到法向量，为式(5)所示，

式中，n为平面的法向量，满足||n||＝1；b为平面到坐标系原点的距离；x_i到平面的距离d_i表示为d_i＝d(x_i)＝n·x_i-b，其中

为Nbhd(x_i)的质心，为了方便计算，假设数据点被平移到原点，令b＝0可以得到式(6)，通过最小化式(6)可以得到n，并满足||n||＝1；

令G＝||n||²-1，将问题转化为在G＝0的约数条件下使得式(6)取得极小值；由拉格朗日乘数法可知取得极小值的点满足式(7)；

其中，λ是拉格朗日乘子，梯度公式的定义为

由式(7)可以得到

其中M表示如下，

于是可以得到M^TM的特征值方程为(M^TM)n＝λn，其中n为M^TM的特征向量，λ为对应的特征值。M^TM的特征值方程可以分解为式(8)表示的形式，

将式(8)的三个分式分别乘上n⁽¹⁾，n⁽²⁾和n⁽³⁾，然后求和可得式(9)，

∑(n·x_i)²＝λ||n||²＝λ (9)

式(9)的左边是目标函数，很容易得到目标函数的极小值为最小特征值，其对应的特征向量就是法向量，假设M^TM有特征向量n₃，则n为n₃或者-n₃，n具体的方向需要通过重新定向得到。

进一步的，所述重定向的步骤如下：

步骤S1.1：构造无向连通图，将1-|n_i·n_j|作为图中边(i,j)的值，容易得出x_i和x_j的距离越小，n_i和n_j就可能满足n_i·n_j≈±1，边的值也就越小；

步骤S1.2：确定初始点，初始点选择x,y,z坐标值达到极大或者极小的点；

步骤S1.3：确定传播方向，按照最小生成树的方向进行传播，如果n_i被确定好，则传向x_j，如果n_i·n_j≤0，则n_j＝-n_j，否则n_j不做改变。

进一步的，所述步骤S3初始配准具体包括一下步骤：

步骤S3.1：采用体素网格滤波对点云模型降采样；

步骤S3.2：选取点p的K近邻中所有的点对p_i和p_j(i≠j)，并估计对应的法向量n_i和n_j；

步骤S3.3：选取点p的K近邻中所有的点对p_i和p_j(i≠j)，并估计对应的法向量n_i和n_j，计算公式如下算公式如下

α＝v·n_j

φ＝(u·(p_j-p_i))/||p_j-p_i||；

θ＝arctan(w·n_j,u·n_j)

步骤S3.4：对于邻域中的每一点，通过式(8)计算点p最终的FPFH

其中，w_k为在给定度量空间中点p与其邻域点的距离；

步骤S3.5：剔除错配对，选择最优的结果为初始配准。

进一步的，所述步骤S3.5中剔除错配对具体为

步骤S3.5.1：从所有m组对应点中从随机抽取3组配对点，并计算刚体变换矩阵；

步骤S3.5.2：对余下的m-3组点变换，若存在一组点的距离小于设定的阈值，则该组点记为内点，反之记为外点；

步骤S3.5.3：重复步骤S3.5.1和步骤S3.5.3，直到达到设定的最大次数时结束，在所有计算结果中选择最优的结果为初始配准。

附图说明

附图1ICP的点到点示意图，图1-(a)ICP的初始位置示意图，图1-(b)ICP的N 次迭代后位置示意图。

附图2ICP的点到面示意图，图2-(a)ICP的初始位置示意图，图2-(b)ICP的N 次迭代后位置示意图。

附图3本发明逐步求精快速配准流程图。

附图4本发明表面法向量的方向示意图，图4-(a)法向量的方向未定向示意图，图4-(b)法向量的方向定向示意图。

附图5本发明点的影响区域示意图。

附图6本发明局部坐标系示意图。

附图7本发明兔子模型初始配准过程示意图，图7-(a)兔子模型初始位置示意图，图7-(b)兔子模型点云精简示意图，图7-(c)兔子模型对应点对选取示意图，图7-(d) 兔子模型配准后位置示意图。

附图8本发明兔子模型配准结果，图8-(a)ICP配准示意图，图8-(b)NICP配准示意图，图8-(c)GICP配准示意图。

附图9本发明兔子模型配准的误差与时间示意图。

附图10本发明风扇盘模型配准结果，图10-(a)风扇盘模型初始位置示意图，图10-(b)风扇盘模型ICP配准示意图。

附图11本发明风扇盘模型配准的误差与时间示意图。

附图12本发明精确配准结果示意图，图12-(a)兔子模型配准结果示意图，图12-(b)风扇盘模型配准结果示意图。

具体实施方式

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

通常标准模型P和待检测模型Q是三维扫描设备在不同的坐标系下获得，在全局坐标系下P和Q没有重合，所以需对两者配准。配准技术将处于不同坐标系下的P和Q通过刚体变换，使得在同一个坐标系下完全重合，刚体变换主要包括旋转和平移，其旋转矩阵R 和平移矩阵T可以表示为：

其中，α,β,γ表示沿x,y,z轴的旋转角，t_x,t_y,t_z表示位移量。若P和Q满足变换(R,T)，则在非齐次坐标形式下的刚体变换为式(1)，

p_i＝Rq_i+T (1)

式中，p_i＝[x′_i,y′_i,z′_i]^T，q_i＝[x_i,y_i,z_i]^T。

ICP算法选取点对是点到点的方法，算法将距离最近的两点作为一组对应点，如图1 所示，该算法在输入的点云模型

和目标点云模型

中，根据点与点之间的欧式距离度量来搜索对应点对(p_i,q_i)，通过最小二乘法计算出R和T，使得误差函数取得极小值。ICP的计算过程为以下4步。

1.分别在点云P和点云Q中选取点p_i∈P和q_i∈Q，使得min||q_i-p_i||；

2.求解得到参数R和T，使得：

3.将点云模型P的点带入p_i＝Rq_i+T，得到模型P′。

4.若达到设定的迭代次数或误差值小于设定的阈值，则停止迭代，否则继续执行1。

NICP是ICP的一种改进算法，通过最小化点到点的距离在其法向量n_i定义的局部表面上的投影来选取对应点,如图3所示，该方法能够降低噪声点对对应点选取的影响，NICP将ICP的步骤2改进为求式(2)的极小值，得到参数R和T，

点到面的方法虽然提高了算法的准确度，但由于假定从输入点云模型中选取的点云代表几何曲面的样本集，仍然会存在离散化带来的差异。面到面的对应点选取方法(GICP) 对ICP算法的步骤2进行了改进，算法假定

和

分别为输入点云P＝{p_i}和目标点云Q＝{q_i}随机选取的对应点集，满足正态分布，即

其中

和

是与对应点相关的协方差矩阵。有相同的下标的

为一组对应点，在确定对应点对后，可以得到一个仿射变换矩阵T，满足q_i＝Tp_i，仿射变换矩阵T通过最小化式(3)得到，

由于假设

和

都是独立的正态分布，d_i ^T是

和

的线性组合，所以也满足正态分布，

于是可以通过最小化误差d_i ^T的负对数似然得到仿射变换矩阵T，如式(4)所示，

一种基于主成分分析的三维点云模型逐步求精快速配准方法，所述逐步求精快速配准方法包括以下步骤，

步骤S1：ICP将初始配准后的点云模型通过PCA方法进行点云精简和法向量估算；

步骤S2：将精简与估算后的点云选取对应点对；

步骤S3：将选取的对应点对进行初始配准后，通过降采样模型的FPFH特征得到初始的变换矩阵，判断误差是否小于初始配准阈值，若小于初始配准阈值，则进行组合迭代精配准，若大于等于初始配准阈值，则返回步骤S2重新选取对应点对；

步骤S4：将进行组合迭代精配准的点对，判断误差是否小于精配准阈值，若大于等于精配准阈值，则返回步骤S2重新选取对应点对，若小于精配准阈值，则配准结束。

进一步的，所述步骤S1的法向量估算为，在点云集合

中找到距离采样点x_i最近的K个相邻的点作为x_i的最近K邻域，记为Nbhd(x_i)，通过Nbhd(x_i) 拟合平面得到法向量，为式(5)所示，

式中，n为平面的法向量，满足||n||＝1；b为平面到坐标系原点的距离；x_i到平面的距离d_i表示为d_i＝d(x_i)＝n·x_i-b，其中

为Nbhd(x_i)的质心，为了方便计算，假设数据点被平移到原点，令b＝0可以得到式(6)，通过最小化式(6)可以得到n，并满足||n||＝1；

令G＝||n||²-1，将问题转化为在G＝0的约数条件下使得式(6)取得极小值；由拉格朗日乘数法可知取得极小值的点满足式(7)；

其中，λ是拉格朗日乘子，梯度公式的定义为

由式(7)可以得到

其中M表示如下，

于是可以得到M^TM的特征值方程为(M^TM)n＝λn，其中n为M^TM的特征向量，λ为对应的特征值。M^TM的特征值方程可以分解为式(8)表示的形式，

将式(8)的三个分式分别乘上n⁽¹⁾，n⁽²⁾和n⁽³⁾，然后求和可得式(9)，

∑(n·x_i)²＝λ||n||²＝λ (9)

式(9)的左边是目标函数，很容易得到目标函数的极小值为最小特征值，其对应的特征向量就是法向量，假设M^TM有特征向量n₃，则n为n₃或者-n₃，n具体的方向需要通过重新定向得到。图4(a)为未定向的情况，图4(b)为重定向的结果。

进一步的，所述重定向的步骤如下：

步骤S1.1：构造无向连通图，将1-|n_i·n_j|作为图中边(i,j)的值，容易得出x_i和x_j的距离越小，n_i和n_j就可能满足n_i·n_j≈±1，边的值也就越小；

步骤S1.2：确定初始点，初始点选择x,y,z坐标值达到极大或者极小的点；

步骤S1.3：确定传播方向，按照最小生成树的方向进行传播，如果n_i被确定好，则传向x_j，如果n_i·n_j≤0，则n_j＝-n_j，否则n_j不做改变。

进一步的，所述步骤S3初始配准具体包括一下步骤：

步骤S3.1：采用体素网格滤波对点云模型降采样；

步骤S3.2：选取点p的K近邻中所有的点对p_i和p_j(i≠j)，并估计对应的法向量n_i和n_j；

步骤S3.3：选取点p的K近邻中所有的点对p_i和p_j(i≠j)，并估计对应的法向量n_i和n_j，计算公式如下算公式如下

α＝v·n_j

φ＝(u·(p_j-p_i))/||p_j-p_i||；

θ＝arctan(w·n_j,u·n_j)

步骤S3.4：对于邻域中的每一点，通过式(8)计算点p最终的FPFH

其中，w_k为在给定度量空间中点p与其邻域点的距离；

步骤S3.5：剔除错配对，选择最优的结果为初始配准。

进一步的，所述步骤S3.5中剔除错配对具体为，

步骤S3.5.1：从所有m组对应点中从随机抽取3组配对点，并计算刚体变换矩阵；

步骤S3.5.2：对余下的m-3组点变换，若存在一组点的距离小于设定的阈值，则该组点记为内点，反之记为外点；

步骤S3.5.3：重复步骤S3.5.1和步骤S3.5.3，直到达到设定的最大次数时结束，在所有计算结果中选择最优的结果为初始配准。

本发明选取全局坐标系下处于不同位置的兔子模型A和B进行配准实验。两个模型如图7(a)所示，逐步求精配准的初始配准参数设置如表1所示，图7(b)为模型的降采样结果；图7(c)为对应点对选取结果的可视化。初始配准得到了两组模型新的位置关系，如图7(d)所示，可以看到未配准前两个兔子模型完全没有重叠部分，配准处理后模型的重叠度明显增大很多并且模型的方向基本一致，但模型之间仍然存在较大的偏差, 配准的误差为1.22778*10^-4，R和T为：

表1初始配准参数设置

初始配准完成后进行精细化配准。为了分析三种不同对应点选取算法的配准效果，本发明对ICP、NICP和GICP的配准结果进行对比分析，如图8所示，蓝色为兔子模型A，红色为兔子模型B，可以看到三组精配准后的兔子模型都几乎完全重合，也就说明配准后的偏差程度较小。为了对三种配准算法进行定量比较，本发明统计了配准过程中误差值与迭代次数以及运行时间与迭代次数的关系，如图9所示，分析图中的曲线，可以看到GICP 对兔子点云的配准时间和误差都要远高于前两种算法，ICP和NICP的配准精度较高，运行时间和迭代收敛时的误差值都比较接近。

以上三种配准算法对两个兔子模型配准，误差函数达到收敛时的迭代次数、最小误差值以及运行时间如表2所示，从表中可以看到ICP对兔子模型配准的精度最高，NICP的运行时间最短，R和T为：

表2兔子模型配准精度对比

为了验证算法的有效性，选用风扇盘模型进行配准，配准结果如图10所示。图10(a) 可以看到第一阶段的初始配准使得两个模型有基本一致的方位，但偏差仍然很大。图10 (b)可以看到，三种配准算法使得模型几乎完全重叠，配准过程中的误差与运行时间如图11所示，可以看到NICP和GICP的配准精度较高，ICP的配准精度最低。表3中可以看到GICP的配准精度最高，但所用时间非常长；用时最短的NICP的配准精度略低。

表3风扇盘模型配准精度对比

通过对比三种精配准算法的配准结果，可以看到单一的精配准算法在处理不同模型时会有比较大的差异。通过实验数据的比较可以发现GICP相对于前两种算法的配准时间比较长，因此，在处理数据规模更大的模型不太高效；ICP的配准稳定性较差，对于不同的模型，配准精度可能很高也可能比较差；NICP的配准时间以及精度都比较稳定，但通过实验发现配准得到的两个模型的位置仍然会在某个方向有细微的错位。因此，本发明采用组合的迭代配准，即在第一阶段的初始配准基础上，第二阶段的精配准先后采用NICP和 ICP进行配准。对兔子模型和风扇盘模型采用组合精配准的结果如图12所示。表4可以看到组合配准算法提高了配准精度，并且运行的时间GICP算法要低很多。

表4组合迭代配准精度

Claims (5)

1.一种基于主成分分析的三维点云模型逐步求精快速配准方法，其特征在于，所述逐步求精快速配准方法包括以下步骤，

步骤S1：ICP将初始配准后的点云模型通过PCA方法进行点云精简和法向量估算；

步骤S2：将精简与估算后的点云选取对应点对；

步骤S3：将选取的对应点对进行初始配准后，通过降采样模型的FPFH特征得到初始的变换矩阵，判断误差是否小于初始配准阈值，若小于初始配准阈值，则进行组合迭代精配准，若大于等于初始配准阈值，则返回步骤S2重新选取对应点对；

步骤S4：将进行组合迭代精配准的点对，判断误差是否小于精配准阈值，若大于等于精配准阈值，则返回步骤S2重新选取对应点对，若小于精配准阈值，则配准结束。

2.根据权利要求1所述一种基于主成分分析的三维点云模型逐步求精快速配准方法，其特征在于，所述步骤S1的法向量估算为，在点云集合

中找到距离采样点x_i最近的K个相邻的点作为x_i的最近K邻域，记为Nbhd(x_i)，通过Nbhd(x_i)拟合平面得到法向量，为式(5)所示，

式中，n为平面的法向量，满足||n||＝1；b为平面到坐标系原点的距离；x_i到平面的距离d_i表示为d_i＝d(x_i)＝n·x_i-b，其中

为Nbhd(x_i)的质心，为了方便计算，假设数据点被平移到原点，令b＝0可以得到式(6)，通过最小化式(6)可以得到n，并满足||n||＝1；

令G＝||n||²-1，将问题转化为在G＝0的约数条件下使得式(6)取得极小值；由拉格朗日乘数法可知取得极小值的点满足式(7)；

▽J＝λ▽G (7)

其中，λ是拉格朗日乘子，梯度公式的定义为

由式(7)可以得到▽J＝λ▽G＝2(M^TM)n，其中M表示如下，

于是可以得到M^TM的特征值方程为(M^TM)n＝λn，其中n为M^TM的特征向量，λ为对应的特征值。M^TM的特征值方程可以分解为式(8)表示的形式，

将式(8)的三个分式分别乘上n⁽¹⁾，n⁽²⁾和n⁽³⁾，然后求和可得式(9)，

∑(n·x_i)²＝λ||n||²＝λ (9)

式(9)的左边是目标函数，很容易得到目标函数的极小值为最小特征值，其对应的特征向量就是法向量，假设M^TM有特征向量n₃，则n为n₃或者-n₃，n具体的方向需要通过重新定向得到。

3.根据权利要求2所述一种基于主成分分析的三维点云模型逐步求精快速配准方法，其特征在于，所述重定向的步骤如下：

步骤S1.1：构造无向连通图，将1-|n_i·n_j|作为图中边(i,j)的值，容易得出x_i和x_j的距离越小，n_i和n_j就可能满足n_i·n_j≈±1，边的值也就越小；

步骤S1.2：确定初始点，初始点选择x,y,z坐标值达到极大或者极小的点；

步骤S1.3：确定传播方向，按照最小生成树的方向进行传播，如果n_i被确定好，则传向x_j，如果n_i·n_j≤0，则n_j＝-n_j，否则n_j不做改变。

4.根据权利要求1所述一种基于主成分分析的三维点云模型逐步求精快速配准方法，其特征在于，所述步骤S3初始配准具体包括一下步骤：

步骤S3.1：采用体素网格滤波对点云模型降采样；

步骤S3.2：选取点p的K近邻中所有的点对p_i和p_j(i≠j)，并估计对应的法向量n_i和n_j；

步骤S3.3：选取点p的K近邻中所有的点对p_i和p_j(i≠j)，并估计对应的法向量n_i和n_j，计算公式如下算公式如下

α＝v·n_j

φ＝(u·(p_j-p_i))/||p_j-p_i||；

θ＝arctan(w·n_j,u·n_j)

步骤S3.4：对于邻域中的每一点，通过式(8)计算点p最终的FPFH

其中，w_k为在给定度量空间中点p与其邻域点的距离；

步骤S3.5：剔除错配对，选择最优的结果为初始配准。

5.根据权利要求4所述一种基于主成分分析的三维点云模型逐步求精快速配准方法，其特征在于，所述步骤S3.5中剔除错配对具体为

步骤S3.5.1：从所有m组对应点中从随机抽取3组配对点，并计算刚体变换矩阵；

步骤S3.5.2：对余下的m-3组点变换，若存在一组点的距离小于设定的阈值，则该组点记为内点，反之记为外点；

步骤S3.5.3：重复步骤S3.5.1和步骤S3.5.3，直到达到设定的最大次数时结束，在所有计算结果中选择最优的结果为初始配准。

Images