CN114170279A - 一种基于激光扫描的点云配准方法 - Google Patents

Abstract

本发明属于三维重建技术领域，具体涉及一种基于激光扫描的点云配准方法。本发明通过体素滤波实现扫描点云数据的精简，减少数据运算量。在点云特征点提取过程中，利用区域分块和法线差异特征进特征点提取，使得提取的特征点可以很好保留点云的几何特征，并在点云模型上分布较为均匀。在点云精配准过程中，利用特征点进行配准，提高点云搜索效率，加快配准速度。针对激光扫描得到得点云数据同一位置点的偏移问题，利用点到面的迭代最近邻点算法进行配准，通过点云到对应点切平面的最小距离构建点对面的匹配误差函数。并且通过法向量夹角约束剔除错误点对，提高配准精度，实现线激光扫描点云的精确配准。

Description

一种基于激光扫描的点云配准方法

技术领域

本发明属于三维重建技术领域，具体涉及一种基于激光扫描的点云配准方法。

背景技术

激光扫描系统被广泛应用于逆向工程、三维重建等领域，为生产实践提供精确的三维数据。但由于激光扫描仪扫描范围有限，常需要对物体进行多角度扫描，通过点云数据配准将不同角度的点云数据转换到同一坐标系下，从而获取物体完整的三维轮廓模型。在点云配准算法中，迭代最近点算法(ICP)最为常用，该方法遍历点云数据中的每一个点对计算欧式距离，通过迭代求取最优变换矩阵，可以得到良好的配准效果。但该方法对点云配准的初值要求较高，且目标函数容易陷入局部最优解，对点云配准的速度和精度有很大的影响，故常需先进行粗配准后再将该算法应用于精配准中。因此本发明提出了一种基于法线差异特征提取与点到面迭代最近邻点算法相结合的点云配准方法，以提高不同角度点云配准精度。

中国专利CN202110581142.1提出了一种改进的ICP点云快速配准算法。该方法利用三对对应点对变换矩阵进行估算，以实现点云数据的粗配准。利用激光扫描仪同轴相机拍摄图像获取点云的旋转变换，仅需对平移向量进行迭代更新求取最终变换矩阵，降低了算法的复杂度，加快点云配准速率。但该方法在粗配准过程中，为提高配准效率仅利用三对对应点进行计算，若三对对应点中存在错误点对将增加配准误差和难度，无法为ICP算法提供一个良好的初值，不利于点云精配准的实现。

中国专利CN201910019924.9提出了一种提升传统ICP算法性能的三维点云自动配准方法。该方法利用KD-tree加速点云的搜索速率，利用法向量和特征直方图实现点云数据的粗配准，达到了有效提取特征点且保证点云特征信息完整的效果。同时利用多分辨率迭代最近点算法，提高了点云配准算法的抗噪性能，改善了点云配准的精度。但该方法并未考虑激光扫描仪每次扫描得到的数据点存在位置偏移问题，使得点云配准不够准确，降低了配准精度。

发明内容

本发明的目的在于提供一种基于激光扫描的点云配准方法。

一种基于激光扫描的点云配准方法，包括以下步骤：

步骤1：激光扫描点云数据下采样精简；

步骤2：基于法线差异特征提取点云邻域特征点，并利用快速点特征直方图对其进行描述；

步骤2.1：将点云进行均匀分块处理，在x轴和y轴方向上分别分为k段，将点云分为k²个大小相同的区域；对任意一个点云p_i，利用主成分分析法分别计算其在小尺度半径r₁邻域和大尺度半径2r₁邻域内的法向量n(p_i,r₁)和n(p_i,2r₁)，由此得到两个不同尺度半径下的法向量的差异特征，公式如下：

通过设定阈值对点云p_i在不同半径邻域内的法向量差异进行判断，实现对点云数据特征点的筛选，并通过点云均匀分块的方式，统计每个区域内提取出的特征点的数量；对于特征点检出少于一定数量的区域，将区域内点云在不同尺度半径下的法向量差异由大到小进行排序，取前10％法向量差异较大的点云为特征点，使得特征点在点云模型上分布较为均衡，解决在较为平缓区域无特征点配准问题；

步骤2.2：对提取得到的特征点云P′和Q′利用快速点特征直方图FPFH进行描述；

其中，ω为权值，一般为特征点P_i′与邻域点P_ij′的欧氏距离；

步骤3：通过采样一致性算法进行点云粗配准，并利用欧式距离约束剔除错误对应点对；

步骤4：对特征点集利用KD-tree算法搜索对应点搜索，通过法向量夹角约束剔除错误点对，采用点到面的迭代最近邻点算法实现点云精配准。

进一步地，所述步骤1中激光扫描点云数据下采样精简的方法具体为：

激光扫描得到的点云数据数量庞大，为数据的后续处理增加了复杂度，首先利用体素栅格滤波对点云数据进行下采样处理，读取点云数据在坐标轴xyz三个方向上的最大坐标值和最小坐标值，由此计算点云数据最小长方体包围盒的边长，包围盒边长分别为：

其中，x_max、x_min、y_max、y_min、z_max、z_min分别为点云数据在x轴、y轴和z轴方向上的最大坐标值和最小坐标值；μ为边长调节因子，以保证点云数据完全被长方体包围盒包围；

根据单位体素内点云数量将包围盒适当地划分成若干个大小相同的体素栅格，划分后将不包含点云的体素栅格进行剔除，计算余下的每个体素栅格的重心，公式如下：

其中，n为体素栅格中点云的数量；

保留每个栅格内距离重心最近的点，来代表体该素栅格内的所有数据，并将栅格内其他点删除，以实现点云数据的下采样；分别对源点云P和目标点云Q进行以上处理，在保证点云数据原有的几何特征的同时，得到均匀精简的效果。

进一步地，所述步骤3中通过采样一致性算法进行点云粗配准，并利用欧式距离约束剔除错误对应点对的方法具体为：在目标点云Q′中寻找与待配准点云P′中采样点FPFH特征最相似的点作为对应点对(P_i′,Q_i′)；通过点对间的欧式距离约束条件剔除错误对应点对，根据正确的对应点对求取旋转矩阵R₀和平移矩阵T₀，并计算对应点对的配准误差，使误差函数取值最小的变换即为对应点对的最优变换，实现点云的初始配准。

进一步地，所述步骤4中对特征点集利用KD-tree算法搜索对应点搜索，通过法向量夹角约束剔除错误点对，采用点到面的迭代最近邻点算法实现点云精配准的方法具体为：

将粗配准过程中求得的旋转矩阵R₀和平移矩阵T₀的初值用于点云数据的精配准中，并且利用提取的特征点集进行配准，减少运算数量；通过KD-tree算法进行最近邻点查找，提高搜索速度；在精配准过程中利用点到面的迭代最近邻点算法，将对应点云法向量与平面的交点作为对应点，构建点到面的匹配误差函数，表达式如下：

其中，n(Q_i′,r₁)为点Q_i′在半径r₁邻域内的法向量；

由于粗配准阶段已经为精配准计算提供了一个良好的初值，使得源点云于目标点云间的角度偏差较小，因此可以将该非线性最小二乘问题简化为线性最小二乘问题，求解平移矩阵T和旋转矩阵R，提高计算速度；同时利用法向量夹角阈值约束对错误对应点进行剔除，约束条件为：

其中，φ为两对应点法向量间的夹角，χ为设定的角度阈值；

由此计算得到旋转矩阵和平移矩阵，并将其继续用于迭代计算，直到误差函数满足设定阈值或达到匹配迭代达到设定次数，则输出最终配准结果；若不满足以上条件，则继续进行迭代运算，得到最优的配准矩阵，实现点云的精确配准。

本发明的有益效果在于：

本发明通过体素滤波实现扫描点云数据的精简，减少数据运算量。在点云特征点提取过程中，利用区域分块和法线差异特征进特征点提取，使得提取的特征点可以很好保留点云的几何特征，并在点云模型上分布较为均匀。在点云精配准过程中，利用特征点进行配准，提高点云搜索效率，加快配准速度。针对激光扫描得到得点云数据同一位置点的偏移问题，利用点到面的迭代最近邻点算法进行配准，通过点云到对应点切平面的最小距离构建点对面的匹配误差函数。并且通过法向量夹角约束剔除错误点对，提高配准精度，实现线激光扫描点云的精确配准。

附图说明

图1为本发明的总体流程图。

图2为本发明中点云法向量夹角特征的示意图。

图3为本发明中局部坐标系的示意图。

图4为本发明中FPFH计算示意图。

图5为本发明中点到面误差计算示意图。

具体实施方式

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

本发明提出了一种基于激光扫描的点云配准方法，针对配准点云仅部分重叠和激光扫描点云数据同一位置上的点存在偏移的情况，通过区域分块和法线差异对点云特征点进行提取，并利用点到面的最近邻点算法实现点云的精配准，提高点云配准的准确率，减小配准误差。

本发明包括以下步骤：(1)激光扫描点云数据下采样精简；(2)基于法线差异特征提取点云邻域特征点，并利用快速点特征直方图对其进行描述；(3)通过采样一致性算法进行点云粗配准，并利用欧式距离约束剔除错误对应点对；(4)对特征点集利用KD-tree算法搜索对应点搜索，通过法向量夹角约束剔除错误点对，采用点到面的迭代最近邻点算法实现点云精配准。

本发明提出的方法通过粗配准和精配准结合的方法对点云数据进行配准，利用法线差异特征提取点云模型特征点，在保证点云几何特征的情况下减少运算数据点。同时使特征点在点云模型上分布较为均衡，在仅有部分重叠点云配准时，避免出现对应特征点数量过少的问题。对于激光扫描初始位置和角度不同而导致两配准点云中同一位置点存在位置偏差，通过点到面的迭代最近邻点算法进行点云的精配准，并利用法向量夹角约束剔除错误点对，提高点云配准的准确性。

本发明的目的通过如下技术方案来实现：

一种基于激光扫描的点云配准方法，包括如下步骤：

步骤1：激光扫描点云数据下采样精简；

步骤2：基于法线差异特征提取点云邻域特征点，并利用快速点特征直方图对其进行描述；

步骤3：通过采样一致性算法进行点云粗配准，并利用欧式距离约束剔除错误对应点对；

步骤4：对特征点集利用KD-tree算法搜索对应点搜索，通过法向量夹角约束剔除错误点对，采用点到面的迭代最近邻点算法实现点云精配准。

上述方案中，所述步骤2通过法线差异特征提取点云邻域特征点，并利用快速点特征直方图对其进行描述，以反映点云数据的几何特征，便于点云数据的粗配准，具体为：

步骤2.1：将点云进行均匀分块处理，在x轴和y轴方向上分别分为k段，将点云分为k²个大小相同的区域。对任意一个点云p_i，利用主成分分析法分别计算其在小尺度半径r₁邻域和大尺度半径2r₁邻域内的法向量n(p_i,r₁)和n(p_i,2r₁)，由此得到两个不同尺度半径下的法向量的差异特征，公式如下：

通过设定阈值对点云p_i在不同半径邻域内的法向量差异进行判断，实现对点云数据特征点的筛选。并通过点云均匀分块的方式，统计每个区域内提取出的特征点的数量。对于特征点检出少于一定数量的区域，将区域内点云在不同尺度半径下的法向量差异由大到小进行排序，取前10％法向量差异较大的点云为特征点，使得特征点在点云模型上分布较为均衡，解决在较为平缓区域无特征点配准问题。

步骤2.2：对提取得到的特征点云P′和Q′利用快速点特征直方图(FPFH)进行描述：

其中，ω为权值，一般为特征点P_i′与邻域点P_ij′的欧氏距离。

上述方案中，所述步骤3通过采样一致性算法进行点云粗配准，并利用欧式距离约束剔除错误对应点对。在目标点云Q′中寻找与待配准点云P′中采样点FPFH特征最相似的点作为对应点对(P_i′,Q_i′)。通过点对间的欧式距离约束条件剔除错误对应点对，根据正确的对应点对求取旋转矩阵R₀和平移矩阵T₀，并计算对应点对的配准误差，使误差函数取值最小的变换即为对应点对的最优变换，实现点云的初始配准。

上述方案中，所述步骤4对特征点集利用KD-tree算法搜索对应点搜索，通过法向量夹角约束剔除错误点对，采用点到面的迭代最近邻点算法实现点云精配准。将粗配准过程中求得的旋转矩阵R₀和平移矩阵T₀的初值用于点云数据的精配准中，并且利用提取的特征点集进行配准，减少运算数量。通过KD-tree算法进行最近邻点查找，提高搜索速度。在精配准过程中利用点到面的迭代最近邻点算法，将对应点云法向量与平面的交点作为对应点，构建点到面的匹配误差函数，表达式如下：

其中，n(Q_i′,r₁)为点Q_i′在半径r₁邻域内的法向量。

由于粗配准阶段已经为精配准计算提供了一个良好的初值，使得源点云于目标点云间的角度偏差较小，因此可以将该非线性最小二乘问题简化为线性最小二乘问题，求解平移矩阵T和旋转矩阵R，提高计算速度。同时利用法向量夹角阈值约束对错误对应点进行剔除，约束条件为：

其中，φ为两对应点法向量间的夹角，χ为设定的角度阈值。

由此计算得到旋转矩阵和平移矩阵，并将其继续用于迭代计算，直到误差函数满足设定阈值或达到匹配迭代达到设定次数，则输出最终配准结果；若不满足以上条件，则继续进行迭代运算，得到最优的配准矩阵，实现点云的精确配准。

实施例1：

激光扫描得到的点云数据数量庞大，为数据的后续处理增加了复杂度，首先利用体素栅格滤波对点云数据进行下采样处理。读取点云数据在坐标轴xyz三个方向上的最大坐标值和最小坐标值，由此计算点云数据最小长方体包围盒的边长，包围盒边长分别为：

其中，x_max、x_min、y_max、y_min、z_max、z_min分别为点云数据在x轴、y轴和z轴方向上的最大坐标值和最小坐标值，μ为边长调节因子，以保证点云数据完全被长方体包围盒包围。

根据单位体素内点云数量将包围盒适当地划分成若干个大小相同的体素栅格，划分后将不包含点云的体素栅格进行剔除，计算余下的每个体素栅格的重心，公式如下：

其中，n为体素栅格中点云的数量。

保留每个栅格内距离重心最近的点，来代表体该素栅格内的所有数据，并将栅格内其他点删除，以实现点云数据的下采样。分别对源点云P和目标点云Q进行以上处理，在保证点云数据原有的几何特征的同时，得到均匀精简的效果。

为了反映点云数据的几何特征，便于点云数据的粗配准，故对精简后的点云数据进行特征点提取。将点云进行均匀分块处理，在x轴和y轴方向上分别分为k段，将点云分为k²个大小相同的区域。剔除不包含点云的区域，计算剩余每个区域内各个点的法向量差异。对任意一个点云p_i利用主成分分析法计算其法向量，取点云p_i在小尺度半径r₁邻域内的k个邻近点p_ij建立协方差矩阵如下：

由此可以求出特征值λ_i1、λ_i2、λ_i3，若λ_i1＜λ_i2＜λ_i3，则特征值λ_i1所对应的特征向量为点云p_i在半径为r₁的邻域内的法向量n(p_i,r₁)。利用相同方法计算点云p_i在大尺度半径2r₁邻域内的法向量n(p_i,2r₁)，由此可以得到两个不同尺度半径下的法向量n(p_i,r₁)和n(p_i,2r₁)的差异特征，公式如下：

通过设定阈值对点云p_i在不同半径邻域内的法向量n(p_i,r₁)和n(p_i,2r₁)的差异进行判断，实现对点云数据特征点的筛选，如图2所示。该方法对起伏较为明显区域可以有效提取出特征点，但对于较为平缓区域点云在不同尺度半径下的法向量差异较小，通过阈值判断将会被滤除。这导致特征点大量集中于起伏较为明显区域，而相对平缓区域特征点无法提取，使得特征点在点云模型上分布不均衡。对于仅存在部分重叠区域的点云模型配准而言，特征点重合较少，增加了配准的难度。因此通过点云均匀分块的方式，统计每个区域内提取出的特征点的数量。对于特征点检出少于一定数量的区域，将区域内点云在不同尺度半径下的法向量差异由大到小进行排序，取前10％法向量差异较大的点云为特征点，解决在较为平缓区域无特征点配准问题。

通过上述方法分别得到两组特征点云P′和Q′，对提取得到的特征点云P′和Q′利用快速点特征直方图(FPFH)进行描述，以特征点云P′为例，对于每个特征点P_i′建立局部坐标系，如图3所示，得到与其邻近点P_ij′间的几何关系。局部坐标系表达式为：

三个角度特征元素表达式为：

其中，n(P_i′,r₁)′和n(P_ij′,r₁)′分别为点P_i′和点P_ij′在半径r₁邻域内的法向量。

将点云与邻域点之间的上述三个角度特征元素称为SPFH(Simple PFH)，再计算每个邻域点与其周围点的特征元素，如图4所示，并利用以下公式计算FPFH特征：

其中，ω为权值，一般为特征点P_i′与邻域点P_ij′的欧氏距离。

通过FPFH描述子分别对两组特征点云P′和Q′进行描述，利用采样一致性算法(SAC-IA)对两组特征点云P′和Q′进行匹配计算。在待配准点云P′中相隔一定距离范围选取采样点，使得特征点FPFH特征有一定的差异，在目标点云Q′中寻找与采样点FPFH特征最相似的点作为对应点对(P_i′,Q_i′)。通过点对间的欧式距离约束对错误对应点对进行剔除，欧式距离约束条件为：

其中，ε为设定阈值。

通过阈值设定，满足欧式距离约束的点对视为正确对应点，并将错误对应点剔除。根据得到的正确对应点对求取旋转矩阵R₀和平移矩阵T₀，并计算对应点对的配准误差，使误差函数取值最小的变换即为对应点对的最优变换，实现点云的初始配准，误差函数如下：

其中，l_i为第i组对应点对变换后的欧式距离，m_l为设定值。

将粗配准过程中求得的旋转矩阵R₀和平移矩阵T₀的初值用于点云数据的精配准中，同时为减少运算数据量，依旧利用提取的特征点集进行精配准计算。在对应点对搜索过程中，通过KD-tree算法进行查找，来提高搜索速度。根据激光扫描数据特点，当物体扫描的初始位置和角度不同时，会导致两个点云模型中同一位置的点存在一定的位置偏差。故在精配准过程中采用点到面的迭代最近邻点算法，如图5所示，利用点云到对应点切平面的最小距离构建点对面的匹配误差函数，表达式如下：

其中，n(Q_i′,r₁)为点Q_i′在半径r₁邻域内的法向量。

上述误差函数为非线性最小二乘问题，求解计算过于复杂，增加了时间的复杂度。由于粗配准阶段已经为精配准计算提供了一个良好的初值，使得源点云于目标点云间的角度偏差较小，因此可以将该误差函数简化为线性最小二乘问题，求解平移矩阵T和旋转矩阵R，提高计算速度。同时利用法向量夹角阈值约束对错误对应点进行剔除，约束条件为：

其中，φ为两对应点法向量间的夹角，χ为设定的角度阈值。

由此计算得到旋转矩阵和平移矩阵，并将其继续用于迭代计算，直到误差函数满足设定阈值或达到匹配迭代达到设定次数，则输出最终配准结果；若不满足以上条件，则继续进行迭代运算，得到最优的配准矩阵，实现点云的精确配准。

本发明通过体素滤波实现扫描点云数据的精简，减少数据运算量。在点云特征点提取过程中，利用区域分块和法线差异特征进特征点提取，使得提取的特征点可以很好保留点云的几何特征，并在点云模型上分布较为均匀。在点云精配准过程中，利用特征点进行配准，提高点云搜索效率，加快配准速度。针对激光扫描得到得点云数据同一位置点的偏移问题，利用点到面的迭代最近邻点算法进行配准，通过点云到对应点切平面的最小距离构建点对面的匹配误差函数。并且通过法向量夹角约束剔除错误点对，提高配准精度，实现线激光扫描点云的精确配准。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于激光扫描的点云配准方法，其特征在于，包括以下步骤：

步骤1：激光扫描点云数据下采样精简；

步骤2：基于法线差异特征提取点云邻域特征点，并利用快速点特征直方图对其进行描述；

步骤2.1：将点云进行均匀分块处理，在x轴和y轴方向上分别分为k段，将点云分为k²个大小相同的区域；对任意一个点云p_i，利用主成分分析法分别计算其在小尺度半径r₁邻域和大尺度半径2r₁邻域内的法向量n(p_i,r₁)和n(p_i,2r₁)，由此得到两个不同尺度半径下的法向量的差异特征，公式如下：

通过设定阈值对点云p_i在不同半径邻域内的法向量差异进行判断，实现对点云数据特征点的筛选，并通过点云均匀分块的方式，统计每个区域内提取出的特征点的数量；对于特征点检出少于一定数量的区域，将区域内点云在不同尺度半径下的法向量差异由大到小进行排序，取前10％法向量差异较大的点云为特征点，使得特征点在点云模型上分布较为均衡，解决在较为平缓区域无特征点配准问题；

步骤2.2：对提取得到的特征点云P′和Q′利用快速点特征直方图FPFH进行描述；

其中，ω为权值，一般为特征点P_i′与邻域点P_ij′的欧氏距离；

步骤3：通过采样一致性算法进行点云粗配准，并利用欧式距离约束剔除错误对应点对；

步骤4：对特征点集利用KD-tree算法搜索对应点搜索，通过法向量夹角约束剔除错误点对，采用点到面的迭代最近邻点算法实现点云精配准。

2.根据权利要求1所述的一种基于激光扫描的点云配准方法，其特征在于：所述步骤1中激光扫描点云数据下采样精简的方法具体为：

激光扫描得到的点云数据数量庞大，为数据的后续处理增加了复杂度，首先利用体素栅格滤波对点云数据进行下采样处理，读取点云数据在坐标轴xyz三个方向上的最大坐标值和最小坐标值，由此计算点云数据最小长方体包围盒的边长，包围盒边长分别为：

其中，x_max、x_min、y_max、y_min、z_max、z_min分别为点云数据在x轴、y轴和z轴方向上的最大坐标值和最小坐标值；μ为边长调节因子，以保证点云数据完全被长方体包围盒包围；

根据单位体素内点云数量将包围盒适当地划分成若干个大小相同的体素栅格，划分后将不包含点云的体素栅格进行剔除，计算余下的每个体素栅格的重心，公式如下：

其中，n为体素栅格中点云的数量；

保留每个栅格内距离重心最近的点，来代表体该素栅格内的所有数据，并将栅格内其他点删除，以实现点云数据的下采样；分别对源点云P和目标点云Q进行以上处理，在保证点云数据原有的几何特征的同时，得到均匀精简的效果。

3.根据权利要求1所述的一种基于激光扫描的点云配准方法，其特征在于：所述步骤3中通过采样一致性算法进行点云粗配准，并利用欧式距离约束剔除错误对应点对的方法具体为：在目标点云Q′中寻找与待配准点云P′中采样点FPFH特征最相似的点作为对应点对(P_i′,Q_i′)；通过点对间的欧式距离约束条件剔除错误对应点对，根据正确的对应点对求取旋转矩阵R₀和平移矩阵T₀，并计算对应点对的配准误差，使误差函数取值最小的变换即为对应点对的最优变换，实现点云的初始配准。

4.根据权利要求1所述的一种基于激光扫描的点云配准方法，其特征在于：所述步骤4中对特征点集利用KD-tree算法搜索对应点搜索，通过法向量夹角约束剔除错误点对，采用点到面的迭代最近邻点算法实现点云精配准的方法具体为：

将粗配准过程中求得的旋转矩阵R₀和平移矩阵T₀的初值用于点云数据的精配准中，并且利用提取的特征点集进行配准，减少运算数量；通过KD-tree算法进行最近邻点查找，提高搜索速度；在精配准过程中利用点到面的迭代最近邻点算法，将对应点云法向量与平面的交点作为对应点，构建点到面的匹配误差函数，表达式如下：

其中，n(Q_i′,r₁)为点Q_i′在半径r₁邻域内的法向量；

由于粗配准阶段已经为精配准计算提供了一个良好的初值，使得源点云于目标点云间的角度偏差较小，因此可以将该非线性最小二乘问题简化为线性最小二乘问题，求解平移矩阵T和旋转矩阵R，提高计算速度；同时利用法向量夹角阈值约束对错误对应点进行剔除，约束条件为：

其中，φ为两对应点法向量间的夹角，χ为设定的角度阈值；

由此计算得到旋转矩阵和平移矩阵，并将其继续用于迭代计算，直到误差函数满足设定阈值或达到匹配迭代达到设定次数，则输出最终配准结果；若不满足以上条件，则继续进行迭代运算，得到最优的配准矩阵，实现点云的精确配准。

Images