CN114677418B - 一种基于点云特征点提取的配准方法 - Google Patents

Abstract

本发明涉及一种基于点云特征点提取的配准方法，属于三维配准技术领域。该方法首先使用改进的体素滤波分别对源点云和目标点云进行下采样处理；其次对下采样后的点云使用内在形状签名特征点检算法提取特征点，得到含有丰富的几何特征信息的点云；然后对提取到的特征点进行描述得到特征描述子；接着使用改进的随机采样一致性算法进行粗配准，获得最佳变换矩阵；最后在最佳变换矩阵的基础上使用基于KD树的迭代最佳点算法进行精配准，通过迭代得到最终的变换矩阵，将源点云转换为目标点云下的坐标系从而完成配准。这种基于点云特征点提取的配准方法配准精度高，配准速度快，鲁棒性强。

Description

一种基于点云特征点提取的配准方法

技术领域

本发明属于三维配准技术领域，具体涉及一种基于点云特征点提取的配准方法。

背景技术

三维点云配准技术是点云数据处理中的重要步骤，也是后续点云表面重建的基础。由于受到采集设备分辨率、环境因素和被测物体表面状况等影响，采集到的点云数据只是覆盖了部分信息，为了获取三维物体表面的完整信息，必须通过点云配准将采集的不同视角的点云数据变换到统一坐标系下。

目前采用的点云配准方法可分为两类：一类是基于优化的方法，另一类是基于特征的方法。基于优化的方法首先是在源点云和目标点云中搜索对应的点对，然后根据对应关系估计变换矩阵，并不断进行迭代，从而找到最佳的变换矩阵。这类方法的局限性在于需要复杂的方法来克服噪声异常值、点云密度变化和配准过程中部分重叠变化，增加了计算的成本。基于特征的方法不直接搜索对应点，该方法首先在目标点云和源点云中分别提取特征点，用其他方法描述这些特征点，利用特征估计变换矩阵，无需迭代。由此可见，特征点的选取及其描述方法将决定配准结果，这类方法往往由于特征点缺乏代表性或特征点数量不足而导致配准精度低。

发明内容

有鉴于此，本发明目的在于：本发明提出一种基于点云特征点提取的配准方法，该配准方法不但提高了配准精度，在内存资源占用率和计算效率方面都有所提高。

为解决上述问题，本发明提供了一种基于点云特征点提取的配准方法，包括以下步骤：

步骤1：对源点云P₀和目标点云Q₀分别使用改进的体素滤波进行下采样，得到下采样后的点云P和点云Q；

步骤2：对点云P和点云Q使用内在形状签名特征点检测算法提取特征点，得到含有特征点的特征点云P_key和特征点云Q_key；

步骤3：针对特征点云P_key和特征点云Q_key中的每个特征点，确定特征点的邻域、邻域加权协方差矩阵和邻域加权协方差矩阵特征向量；

所述特征点邻域具体为：以特征点k为中心且半径为R的球形邻域确定为特征点k的特征点邻域；

所述特征点邻域加权协方差矩阵的计算公式如下所示：

其中，k_s是特征点k的特征点邻域内的任意一点，s是满足||k_s-k||≤R条件的数，d_s＝||k_s-k||₂，M为特征点k的邻域加权协方差矩阵；

步骤4：根据步骤3得到的邻域加权协方差矩阵特征向量确定各个特征点的局部参考坐标系，沿着所述局部参考坐标系的三个坐标轴方向将特征点周围的球形邻域通过一个球面网格进行划分，得到特征点局部区域，计算特征点局部区域的特征描述子，所述特征描述子为所述特征点局部区域内的点数加权和，所述加权的权重公式如下所示：

其中，k_j是所述特征点局部区域内的任意一点，V(x,y,z)表示所述特征点局部区域内的体积，ρ_j是所述特征点局部区域内的点的密度；

步骤5：基于所述特征描述子，使用改进的随机采样一致性算法对特征点云P_key和特征点云Q_key进行粗配准，完成初始配准，得到最佳变换矩阵

步骤6：在最佳变换矩阵的基础上使用基于KD树的迭代最佳点算法进行精配准，通过不断修正源点云和目标点云的刚体变换，将源点云转换为目标点云下的坐标系从而完成配准。

进一步的，步骤1中，所述对源点云P₀和目标点云Q₀分别使用改进的体素滤波进行下采样，具体包括以下步骤：

步骤1.1：对源点云P₀和目标点云Q₀创建一个三维体素栅格；

步骤1.2：在每个体素内用体素重心点近似表示体素中的其他点，得到体素滤波下采样的结果；所述体素重心点为体素中所有点的重心；

步骤1.3：根据步骤1.2得到的体素滤波下采样的结果，用最近邻搜索算法分别对源点云P₀和目标点云Q₀中的点云数据进行搜索，找到距离体素重心点最近的点代替该体素重心点，最后得到改进的体素滤波下采样后的点云P和点云Q。

进一步的，步骤2中，对所述点云P和点云Q使用内在形状签名特征点检测算法提取特征点，具体包括以下步骤：

步骤2.1：针对点云P和点云Q中的每个特征点p_i，设定一个搜索半径r_ISS，并求特征点p_i的协方差矩阵cov(p_i)，如下式所示：

其中，p_j是以p_i为中心，r_ISS为半径区域内的任意一点，w_ij是以p_i为中心，r_ISS为半径区域内所有点的权值，其计算公式为：

步骤2.2：计算每个特征点p_i的协方差矩阵cov(p_i)的特征值并将按大小递减排列，然后将满足下式的特征点视为内部形状签名候选特征点：

其中，ε₁和ε₂为预设的阈值，0＜ε₁,ε₂＜1；

步骤2.3：对所述内部形状签名候选特征点进行非极大值抑制策略筛选出最终的特征点，得到含有特征点的点云P_key和Q_key。

进一步的，步骤5中，使用改进的随机采样一致性算法对特征点云P_key和特征点云Q_key进行粗配准，具体包括以下步骤：

步骤5.1：特征点云P_key中选择n个随机采样点p_{key_1},p_{key_2},...,p_{key_n}，根据所述特征描述子通过最近邻匹配搜索Q_key中的n个对应点q_{key_1},q_{key_2},...,q_{key_n}，得到n个对应点对(p_{key_1},q_{key_1}),(p_{key_2},q_{key_2}),...,(p_{key_n},q_{key_n})，其中，n≥3；

步骤5.2：计算n个随机采样点在各自空间中的欧式距离，n个随机采样点会形成多条虚拟多边形的边长，根据如下公式计算各条虚拟多边形边长之间的相异向量然后将和预设的边长相似度阈值ε_ploy进行比较，如果/>则继续进行下一步，否则返回步骤5.1；

其中，表示p_{key_a}与p_{key_b}之间的欧式距离，/>表示q_{key_a}与q_{key_b}之间的欧式距离；

步骤5.3：用n个对应点对(p_{key_1},q_{key_1}),(p_{key_2},q_{key_2}),...,(p_{key_n},q_{key_n})估计变换矩阵

步骤5.4：根据变换矩阵将特征点云P_key变换为点云P′_key；计算点云P′_key中的各个点和特征点云Q_key之间的最近邻的欧氏距离，并获取点云P′_key中最近邻距离小于预设的距离阈值ε_ransac的点作为内点，计算内点与对应点之间的距离/>p′_{key_c}表示点云P′_key中获取的内点，q_{key_c}是各个内点在Q_key中的对应点，如果内点的数量少于特征点云P_key中点数的四分之一，返回到步骤5.1；

步骤5.5：根据内点和特征点云Q_key中对应点关系重新计算变换矩阵转到步骤5.4；

步骤5.6：当达到最小值或者达到最大迭代次数t_RANSAC时停止迭代，此时获得最佳变换矩阵/>所述最大迭代次数t_RANSAC的计算公式如下所示：

其中，ρ为给定的期望成功概率，ω为期望内联分数。

进一步的，在步骤6中，在最佳变换矩阵的基础上使用基于KD树的迭代最佳点算法进行精配准，具体包括以下步骤：

步骤6.1：采用最佳变换矩阵对特征点云P_key中的每一个点p_{key_i}进行变换，得到点云p″_key，对点云p″_key中的每个点p″_{key_i}使用KD树算法搜索特征点云Q_key中距离点p″_{key_i}最近的点q_{key_i}，形成对应点对(p″_{key_i},q_{key_i})；

步骤6.2：根据上述的对应点对，用最小二乘法迭代计算旋转矩阵R和平移矩阵T，使误差函数G(R,T)最小化，误差函数的计算公式如下所示：

其中，N是上述对应点对的个数，p″_{key_i}是点云P″_key中的任意一点，q_{key_i}是点云Q_key中找到的对应点；

步骤6.3：采用步骤6.2得到的旋转矩阵R和平移矩阵T对点云P″_key中的每一个点p″_{key_i}进行变换，得到变换后的对应点p″′_{key_i}，同时计算点对(p″′_{key_i},q_{key_i})的平均距离D，计算公式如下所示：

步骤6.4：设定阈值ε_icp，如果满足D_m-D_m+1＜ε_icp结束迭代，D_m为第m次迭代的平均距离，D_m+1为第m+1次迭代的平均距离；否则转到步骤6.1改变最佳变换矩阵，此时最佳变换矩阵为T₀*R*T；

步骤6.5：根据最终获得的旋转矩阵旋转矩阵R和平移矩阵T，将源点云P₀转化为目标点云Q₀的坐标系下从而完成配准。

发明优点

本发明融合唯一形状上下文的点云配准方法，具有以下优点：

(1)使用改进的体素滤波，用原始点云数据中距离体素重心点最近的点代替该重心点，保存了原始点云的细小特征，提高了点云数据的准确性，对于采样点对应曲面的表示更为准确。

(2)使用特征点形状描述子只需要在每个特征点上计算一个确定的描述子而不需要计算多个模糊的描述子，减少了计算量。

(3)使用改进的随机采样一致性算法，加入预排除步骤，可以立即过滤掉错误的姿态假设，从而节省时间来生成更多其他可能正确的假设姿态，提高了配准时间。

附图说明

图1为本发明方法的bunny点云配准图；

图2为本发明方法的dragon点云配准图；

图3为不使用改进的体素滤波bunny点云配准图。

具体实施方式

下面结合附图和具体实例对本发明的技术方案做进一步地详细说明。

一种基于点云特征点提取的配准方法的硬件环境为Intel(R)Core(TM)[email protected]处理器，***环境为Windows10企业版，开发环境为PCL1.8.0和visualstudio2013，实验使用的点云模型为斯坦福大学点云库bunny和dragon进行配准。不同的点云模型所设参数不同，下面以bunny模型为例，基于点云特征点提取的配准方法，具体按照以下步骤实施：

步骤1：首先对源点云P₀和目标点云Q₀创建一个三维体素栅格，定义参数float_step，用setLeafSize(_step,_step,_step)来设置体素栅格的边长，本实例中_step＝0.001；其次在每个体素内用体素中所有点的重心近似表示体素中的其他点得到体素滤波下采样的结果；然后用最近邻搜索算法分别对源点云P₀和目标点云Q₀中的点云数据进行搜索，找到距离体素重心点最近的点代替该体素重心点；最后得到改进的体素滤波下采样后的点云P和点云Q。

步骤2：对步骤1得到的点云P和Q使用内在形状签名特征点检测算法提取特征点，具体包括以下步骤：

步骤2.1，针对下采样后的点云P和Q中的每个点p_i，设定一个搜索半径r_ISS，本实例中r_ISS＝0.01，并求特征点的p_i协方差矩阵cov(p_i)，如下式所示：

其中，p_j是以p_i为中心，r_ISS为半径区域内的任意一点，w_ij是以p_i为中心，r_ISS为半径区域内所有点的权值，其计算公式为：

步骤2.2，计算每个特征点p_i的协方差矩阵cov(p_i)的特征值并将按大小递减排列，然后将满足下式的特征点视为内部形状签名候选特征点：

其中，ε₁和ε₂为预设的阈值，0＜ε₁,ε₂＜1，本实例中ε₁和ε₂都设置为0.975；

步骤2.3，对部形状签名候选特征点进行非极大值抑制策略筛选出最终的特征点，定义参数_NonMaxRadius，本实例中_NonMaxRadius＝0.02，用setNonMaxRadius(_NonMaxRadius)来设置非极大抑制半径，从而保留沿主方向变化较大的特征点为最终提取的特征点，得到含有特征点的点云P_key和Q_key。

步骤3：针对步骤2提取到的特征点云即P_key和Q_key中每个特征点确定其对应的邻域，所述邻域为以特征点k为中心，半径为R的球形邻域，本实例中R＝0.03，并根据如下公式计算所述邻域内点的加权协方差矩阵，得到对应的邻域加权协方差矩阵M：

其中，k_s是特征点k的特征点邻域内的任意一点，s是满足||k_s-k||≤R条件的数，d_s＝||k_s-k||，

步骤4：通过上述协方差矩阵M，计算其特征向量并进行特征向量的总体最小二乘估计获得各个特征点唯一且确定的局部参考坐标系，沿着局部参考坐标系的三个坐标轴方向将特征点周围的球形邻域通过一个球面网格进行唯一划分，划分后计算特征描述子即统计每个网格内的点数加权和，权重公式如下所示：

其中，k_j是划分的网格内的任意一点，V(x,y,z)表示局部参考坐标系产生的三个坐标轴方向区域对应的体积，ρ_j是对应体积内的局部点的密度。

步骤5：基于上述特征描述子，使用改进的随机采样一致性算法对特征点云P_key和Q_key进行粗配准，具体包括以下步骤：

步骤5.1：特征点云P_key中选择n个随机采样点p_{key_1},p_{key_2},...,p_{key_n}，根据特征描述子通过最近邻匹配搜索Q_key中的n个对应点q_{key_1},q_{key_2},...,q_{key_n}，得到n个对应点对(p_{key_1},q_{key_1}),(p_{key_2},q_{key_2}),...,(p_{key_n},q_{key_n})，其中，n≥3；

步骤5.2，对随机采样的点进行预排除，首先计算n个采样点在各自空间中的欧式距离，n个采样点会形成多条虚拟多边形的边长，计算多边形边长之间的相异向量其次将/>和边长相似度阈值ε_ploy进行比较，如果小于阈值则继续进行下一步，否则返回步骤5.1，本示例中n＝3，/>的计算公式如下所示：

其中是点云P_key中第一个采样点和第二个采样点之间的欧式距离，/>是点云Q_key中对应的第一个采样点和第二个采样点之间的欧式距离，其他类似；

步骤5.3，用n个对应点对(p_{key_1},q_{key_1}),(p_{key_2},q_{key_2}),...,(p_{key_n},q_{key_n})估计变换矩阵

步骤5.4，根据变换矩阵将特征点云P_key变换为点云P′_key；计算点云P′_key中的各个点和特征点云Q_key之间的最近邻的欧氏距离，并获取点云P′_key中最近邻距离小于预设的距离阈值ε_ransac的点作为内点，计算内点与对应点之间的距离/>p′_{key_c}表示点云P′_key中获取的内点，q′_{key_c}是各个内点在Q_key中的对应点，如果内点的数量少于特征点云P_key中点数的四分之一，返回到步骤5.1；定义一个参数float_Fraction，用setInlierFraction(_Fraction)来设置可以接受位姿假设所需的内点比例，本实例中ε_ransac＝0.0125，_Fraction＝0.25；

步骤5.5，根据内点和特征点云Q_key中对应点关系重新计算变换矩阵转到步骤5.4；

步骤5.6，当达到最小值或者达到最大迭代次数t_RANSAC时停止迭代，此时获得最佳变换矩阵/>随机采样一致性的粗配准的迭代次数t_RANSAC由给定的期望成功概率ρ和期望内联分数ω所得，计算公式如下所示：

步骤6：在步骤5最佳变换矩阵基础上使用基于KD树的迭代最佳点(ICP)算法进行精配准，通过不断迭代最佳点算法修正源点云和目标点云的刚体变换，将源点云转换为目标点云下的坐标系从而完成配准。具体包括以下步骤：

步骤6.1，采用最佳变换矩阵对特征点云P_key中的每一个点p_{key_i}进行变换，得到点云p″_key，对点云p″_key中的每个点p″_{key_i}使用KD树算法搜索特征点云Q_key中距离点p″_{key_i}最近的点q_{key_i}，形成对应点对(p″_{key_i},q_{key_i})；

步骤6.2，根据上述的对应点对，用最小二乘法迭代计算旋转矩阵R和平移矩阵T，使误差函数G(R,T)最小化，误差函数的计算公式如下所示：

其中，N是上述对应点对的个数，p″_{key_i}是点云P″_key中的任意一点，q_{key_i}是点云Q_key中找到的对应点；

步骤6.3，采用步骤6.2得到的旋转矩阵R和平移矩阵T对点云P″_key中的每一个点p″_{key_i}进行变换，得到变换后的对应点p″′_{key_i}，同时计算点对(p″′_{key_i},q_{key_i})的平均距离D，计算公式如下所示：

步骤6.4，设定阈值ε_icp，如果满足D_m-D_m+1＜ε_icp结束迭代，D_m为第m次迭代的平均距离，D_m+1为第m+1次迭代的平均距离；否则转到步骤6.1改变最佳变换矩阵，此时最佳变换矩阵为T₀*R*T，本实例中ε_icp＝0.04；

步骤6.5，根据最终获得的旋转矩阵旋转矩阵R和平移矩阵T，将源点云P₀转化为目标点云Q₀的坐标系下从而完成配准。

通过上述配准方法可以得到图1和图2配准图，可以看出配准后的点云和目标点云几乎重合，配准精度较高。再将本文的方法和利用三维形状上下文特征描述子进行对比分析，并且控制粗配准和精配准的参数相同，给出各个算法的总时间、粗配准时间、精配准时间和配准误差，下表分别为bunny点云数据和dragon点云数据的配准结果。

通过以上表格可以说明本发明方法与利用三维形状上下文特征描述子的配准方法相比，在对bunny和dragon点云数据进行配准时配准时间缩短，一定范围内也提高了配准精度。另外使用改进的体素滤波在一定程度上也可以提高配准精度，如图3，在bunny点云数据配准中使用三维形状上下文描述子时不使用改进的体素滤波无法精确配准。因此本发明提出的基于点云特征点提取的配准方法配准精度高，配算速度快，鲁棒性强。

Claims (3)

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

步骤1：对源点云P₀和目标点云Q₀分别使用改进的体素滤波进行下采样，得到下采样后的点云P和点云Q；

其中，所述对源点云P₀和目标点云Q₀分别使用改进的体素滤波进行下采样，具体包括以下步骤：

步骤1.1：对源点云P₀和目标点云Q₀创建一个三维体素栅格；

步骤1.2：在每个体素内用体素重心点近似表示体素中的其他点，得到体素滤波下采样的结果；所述体素重心点为体素中所有点的重心；

步骤1.3：根据步骤1.2得到的体素滤波下采样的结果，用最近邻搜索算法分别对源点云P₀和目标点云Q₀中的点云数据进行搜索，找到距离体素重心点最近的点代替该体素重心点，最后得到改进的体素滤波下采样后的点云P和点云Q；

步骤2：对点云P和点云Q使用内在形状签名特征点检测算法提取特征点，得到含有特征点的特征点云P_key和特征点云Q_key；

其中，对所述点云P和点云Q使用内在形状签名特征点检测算法提取特征点，具体包括以下步骤：

步骤2.1：针对点云P和点云Q中的每个特征点p_i，设定一个搜索半径r_ISS，并求特征点p_i的协方差矩阵cov(p_i)，如下式所示：

其中，p_j是以p_i为中心，r_ISS为半径区域内的任意一点，w_ij是以p_i为中心，r_ISS为半径区域内所有点的权值，其计算公式为：

步骤2.2：计算每个特征点p_i的协方差矩阵cov(p_i)的特征值并将/>按大小递减排列，然后将满足下式的特征点视为内部形状签名候选特征点：

其中，ε₁和ε₂为预设的阈值，0＜ε₁,ε₂＜1；

步骤2.3：对所述内部形状签名候选特征点进行非极大值抑制策略筛选出最终的特征点，得到含有特征点的点云P_key和Q_key；

步骤3：针对特征点云P_key和特征点云Q_key中的每个特征点，确定特征点的邻域、邻域加权协方差矩阵和邻域加权协方差矩阵特征向量；

所述特征点邻域具体为：以特征点k为中心且半径为R的球形邻域确定为特征点k的特征点邻域；

所述特征点邻域加权协方差矩阵的计算公式如下所示：

其中，k_s是特征点k的特征点邻域内的任意一点，s是满足||k_s-k||≤R条件的数，d_s＝||k_s-k||₂，M为特征点k的邻域加权协方差矩阵；

步骤4：根据步骤3得到的邻域加权协方差矩阵特征向量确定各个特征点的局部参考坐标系，沿着所述局部参考坐标系的三个坐标轴方向将特征点周围的球形邻域通过一个球面网格进行划分，得到特征点局部区域，计算特征点局部区域的特征描述子，所述特征描述子为所述特征点局部区域内的点数加权和，所述加权的权重公式如下所示：

其中，k_j是所述特征点局部区域内的任意一点，V(x,y,z)表示所述特征点局部区域内的体积，ρ_j是所述特征点局部区域内的点的密度；

步骤5：基于所述特征描述子，使用改进的随机采样一致性算法对特征点云P_key和特征点云Q_key进行粗配准，完成初始配准，得到最佳变换矩阵

步骤6：在最佳变换矩阵的基础上使用基于KD树的迭代最佳点算法进行精配准，通过不断修正源点云和目标点云的刚体变换，将源点云转换为目标点云下的坐标系从而完成配准。

2.如权利要求1所述的配准方法，其特征在于，步骤5中，使用改进的随机采样一致性算法对特征点云P_key和特征点云Q_key进行粗配准，具体包括以下步骤：

步骤5.1：特征点云P_key中选择n个随机采样点p_{key_1},p_{key_2},...,p_{key_n}，根据所述特征描述子通过最近邻匹配搜索Q_key中的n个对应点q_{key_1},q_{key_2},...,q_{key_n}，得到n个对应点对(p_{key_1},q_{key_1}),(p_{key_2},q_{key_2}),...,(p_{key_n},q_{key_n})，其中，n≥3；

步骤5.2：计算n个随机采样点在各自空间中的欧式距离，n个随机采样点会形成多条虚拟多边形的边长，根据如下公式计算各条虚拟多边形边长之间的相异向量然后将/>和预设的边长相似度阈值ε_ploy进行比较，如果/>则继续进行下一步，否则返回步骤5.1；

其中，表示p_{key_a}与p_{key_b}之间的欧式距离，/>表示q_{key_a}与q_{key_b}之间的欧式距离；

步骤5.3：用n个对应点对(p_{key_1},q_{key_1}),(p_{key_2},q_{key_2}),...,(p_{key_n},q_{key_n})估计变换矩阵

步骤5.4：根据变换矩阵将特征点云P_key变换为点云P′_key；计算点云P′_key中的各个点和特征点云Q_key之间的最近邻的欧氏距离，并获取点云P′_key中最近邻距离小于预设的距离阈值ε_ransac的点作为内点，计算内点与对应点之间的距离/>p′_{key_c}表示点云P′_key中获取的内点，q_{key_c}是各个内点在Q_key中的对应点，如果内点的数量少于特征点云P_key中点数的四分之一，返回到步骤5.1；

步骤5.5：根据内点和特征点云Q_key中对应点关系重新计算变换矩阵转到步骤5.4；

步骤5.6：当达到最小值或者达到最大迭代次数t_RANSAC时停止迭代，此时获得最佳变换矩阵/>所述最大迭代次数t_RANSAC的计算公式如下所示：

其中，ρ为给定的期望成功概率，ω为期望内联分数。

3.如权利要求1所述的配准方法，其特征在于，在步骤6中，在最佳变换矩阵的基础上使用基于KD树的迭代最佳点算法进行精配准，具体包括以下步骤：

步骤6.1：采用最佳变换矩阵对特征点云P_key中的每一个点p_{key_i}进行变换，得到点云p″_key，对点云p″_key中的每个点p″_{key_i}使用KD树算法搜索特征点云Q_key中距离点p″_{key_i}最近的点q_{key_i}，形成对应点对(p″_{key_i},q_{key_i})；

步骤6.2：根据上述的对应点对，用最小二乘法迭代计算旋转矩阵R和平移矩阵T，使误差函数G(R,T)最小化，误差函数的计算公式如下所示：

其中，N是上述对应点对的个数，p″_{key_i}是点云P″_key中的任意一点，q_{key_i}是点云Q_key中找到的对应点；

步骤6.3：采用步骤6.2得到的旋转矩阵R和平移矩阵T对点云P″_key中的每一个点p″_{key_i}进行变换，得到变换后的对应点p″′_{key_i}，p″′_{key_i}＝R_{p′k′ey_i}+T,p″_{key_i}∈P″_key，同时计算点对(p″′_{key_i},q_{key_i})的平均距离D，计算公式如下所示：

步骤6.4：设定阈值ε_icp，如果满足D_m-D_m+1＜ε_icp结束迭代，D_m为第m次迭代的平均距离，D_m+1为第m+1次迭代的平均距离；否则转到步骤6.1改变最佳变换矩阵，此时最佳变换矩阵为T₀*R*T；

步骤6.5：根据最终获得的旋转矩阵旋转矩阵R和平移矩阵T，将源点云P₀转化为目标点云Q₀的坐标系下从而完成配准。