一种改进的ICP点云自动配准方法与流程

本发明属于三维建模技术领域，具体涉及一种改进的ICP点云自动配准方法。

背景技术：

随着三维激光扫描技术的发展与普及，计算机辅助设计和图形图像科学技术的快速发展。三维测量系统具有了全天候、高精度、三维立体可视化等超越传统测量的优势，备受广大测量人士的青睐。它不仅融合了计算机技术、图形图像学等新鲜学科的新技术，而且对测量数据完成了从二维到三维的变革，同时又能为我们提供物体表面的纹理信息并以最真实的形态对被测物体进行虚拟现实重现，并以电子数据的形式进行保存。

将配准技术推向自动化的是计算机视觉研究者Besl和Mckay，他们提出了这样一种算法：基于自由形态曲面进行配准，也被称为最近点迭代法：ICP(Iterative Closest Point)。对于最初的ICP算法而言，并不适用于大部分情况的点云数据，例如：(1)ICP算法对初值的要求比较高，需要两点云之间确定一个良好的初始姿态，否则会造成迭代不收敛或陷入局部最优解的情况，最终导致误配准或不收敛。(2)ICP算法随着大数据时代的开启，往往点云数量动辄上百万、千万，这样再利用该算法在整个点云内逐点进行搜索会造成计算速度缓慢。(3)该算法由于只约束了点对点的距离条件，由于噪声的存在会出现许多误配准点对，而原始ICP算法并未对误配准点对进行有效的剔除。这样的一些不足也为未来点云自动配准算法的改进指明了方向。

技术实现要素：

针对现有技术的不足，本发明提出一种改进的ICP点云自动配准方法。

本发明的技术方案是：

一种改进的ICP点云自动配准方法，包括以下步骤：

步骤1：获取三维物体的具有重叠部分的两组三维点云数据；

步骤2：基于边界特征点云对两组点云数据进行自动粗配准；

步骤2.1：分别创建两组点云的最小包围盒，根据点云密度的k倍进行包围盒网格划分，沿三个轴向分别对包围盒进行等间距划分，对点云数据进行空间格网化：即含有数据点的网格为实格，不含有任何数据点的网格为空格；

所述创建两组点云的最小包围盒采用OBB包围盒法。

步骤2.2：根据网格周围相邻网格中空格数量确立约束条件，进行边界种子网格提取，并采用生长算法得到两组点云数据的所有边界网格；

步骤2.2.1：用方向向量确定空间格网化后的网格周围的六个相邻网格，对网格进行二值化处理；

步骤2.2.2：将空间格网化后的网格周围六个相邻网格中空格数量为3、4、5的网格作为边界种子网格，进行边界种子网格提取；

步骤2.2.3：采用生长算法对边界种子网格进行生长，得到两组点云数据的所有边界网格。

步骤2.3：利用边界网格提取两组点云数据的边界点云特征点集S＝(s₁，s₂，...s_i，...s_I)和T＝(t₁，t₂，...t_j，...t_J)；

步骤2.4：利用多尺度特征法从两组点云数据的边界点云特征点中确定相应的配准点对，得到配准点集P₁＝(p₁，p₂，...p_i，...p_n)、Q₁＝(q₁，q₂，...q_j，...q_m)；

步骤2.5：设定配准点距离阈值d_th和配准点角度阈值θ_th，对配准点集P₁＝(p₁，p₂，...p_i，...p_n)、Q₁＝(q₁，q₂，...q_j，...q_m)中的配准点进行误差剔除：对于配准点集P₁＝(p₁，p₂，...p_i，...p_n)、Q₁＝(q₁，q₂，...q_j，...q_m)中的配准点对{p_i，q_j}，{p_i+1，q_j+1}，{p_i+2，q_j+2}当||p_ip_i+1-q_jq_j+1||≤d_th、||p_i+1p_i+2-q_j+1q_j+2||≤d_th，且||θ₁-θ₂||≤θ_th，则保留该配准点对，否则，删除其配准点对，得到粗配准的配准点集P′₁＝(p₁，p₂，...p_i′，...p_n′)、Q′₁＝(q₁，q₂，...q_j′，...q_m′)，其中，θ₁为p_ip_i+1与p_i+1p_i+2的夹角，θ₂为q_iq_i+1与q_i+1q_i+2的夹角；

采用四元数法确定得到粗配准的配准点集中的配准点对的坐标转换系数，以其中一组点云数据作为目标点云，对两组点云进行初始配准；

步骤3：采用改进的ICP算法对初始配准的点云数据进行精配准；

步骤3.1：针对粗配准的配准点集P′₁＝(p₁，p₂，...p_i′，...p_n′)中任一数据点p_i′，采用kd-tree算法以最近点原则找到在粗配准的配准点集Q′₁＝(q₁，q₂，...q_j′，...q_m′)中距离数据点p_i′最近的三个数据点

步骤3.2：将数据点构成三角形，以数据点p_i′向所构成三角形的平面作垂线，得到垂足q_j′；

步骤3.3：若构成的三角形为非钝角三角形，则垂足q_j′位于三角形中，保留该配准点对，否则，将顶点与三角形的钝角点组成配准点对；

步骤3.4：重复步骤3.1-步骤3.3，直至得到精配准的配准点集；

步骤4：采用CPC三约束法剔除精配准的配准点集中的误配准点对，得到剔除误差的精配准的配准点对；

步骤5：根据求得的剔除误差的精配准的配准点对求解转换矩阵，对两组点云进行精配准，得到配准结果。

所述CPC三约束法包括：方向约束剔除错误点对、刚性约束剔除错误点对、配准误差剔除错误点对。

所述采用生长算法对边界种子网格进行生长，得到两组点云数据的所有边界网格的具体过程如下所示：

建立边界种子网格与网格之间的拓扑关系函数和拓扑关系连接数目函数，采用生长算法优先沿边界种子网格的面作为拓扑进行生长，然后依次按照边界种子网格的边、边界种子网格的顶点作为拓扑方向进行生长，得到两组点云数据的所有边界网格。

本发明的有益效果：

本发明提出一种改进的ICP点云自动配准方法，本发明方法针对传统的点云自动粗配准算法对重叠度低和重叠部分几何特征不明显的点云数据适用性不强问题，本发明结合点云特征提取技术与边界提取技术，提出了“基于边界特征点云”自动粗配准算法，增强了重叠度低和重叠部分几何特征不明显的点云数据粗配准适用性；本发明利用点到最近三点组成的面的垂足作为最近点，并且以这个垂距作为迭代约束函数的理论，对算法进行了分类约束改进，剔除了点落在面外造成虚假配准的情况，提高了配准精度；本发明综合了多种利用阈值来约束点云质量的方法，提出了基于配准根本理论CPC(最近点搜索)中存在的几何约束对配准点集进行错误点对剔除。对点对的剔除量达到了整个配准点对数量的1/3，增强了自动配准技术的鲁棒性和抗噪声能力。

附图说明

图1为本发明具体实施方式中改进的ICP点云自动配准方法的流程图；

图2为本发明具体实施方式中配准点集中各数据点构成的三角形示意图；

图3为本发明具体实施方式中配准点集P′₁中任一数据点与距离配准点集Q′₁中最近的三个数据点形成的锥体示意图；

其中，(a)第一种情况，(b)第二种情况，(c)第三种情况；

图4为本发明具体实施方式中锥体中全为锐角、存在直角、底面三角形为钝角三角形的锥体示意图；

其中，(a)存在直角的锥体示意图，(b)全为锐角的锥体示意图，(c)底面三角形为钝角三角形的锥体示意图；

图5为本发明具体实施方式中当六个角中存在钝角θ＞90°时锥形模型。

具体实施方式

下面结合附图对本发明具体实施方式加以详细的说明。

一种改进的ICP点云自动配准方法，如图1所示，包括以下步骤：

步骤1：获取三维物体的具有重叠部分的两组三维点云数据。

步骤2：基于边界特征点云对两组点云数据进行自动粗配准。

步骤2.1：分别创建两组点云的最小包围盒，根据点云密度的k倍进行包围盒网格划分，沿三个轴向分别对包围盒进行等间距划分，对点云数据进行空间格网化：即含有数据点的网格为实格，不含有任何数据点的网格为空格。

本实施方式中，创建两组点云的最小包围盒采用OBB包围盒法。通过实验得出，在点云总量在百万到千万之间时，k的取值在10-22之间能得到比较好的结果。

步骤2.2：根据网格周围相邻网格中空格数量确立约束条件，进行边界种子网格提取，并采用生长算法得到两组点云数据的所有边界网格。

步骤2.2.1：用方向向量确定空间格网化后的网格周围的六个相邻网格，对网格进行二值化处理。

本实施方式中，根据网格的结构特点，用方向向量找到与其相联系的六个网格的位置，对网格进行二值化处理，设置二值化函数f(x，y，z)，其中，(x，y，z)为网格坐标，对于分化出的网格，若为实格，即含有点云数据，则f(x，y，z)＝1，若为空格，即不含点云数据或已经超出网格范围则f(x，y，z)＝0。

步骤2.2.2：将空间格网化后的网格周围六个相邻网格中空格数量为3、4、5的网格作为边界种子网格，进行边界种子网格提取。

本实施方式中，设种子网格判断函数为g(x，y，z)，根据网格的拓扑方向向量(i，j，k)可知：[-1≤i≤1，-1≤j≤1，-1≤k≤1|i+j+k＝±1∩(i＝0∪j＝0∪k＝0)]，得到种子网格判断方程如式(1)所示：

g(x，y，z)＝∑_-1≤i≤1∑_-1≤j≤1∑_-1≤k≤1|f(x，y，z)-f(x+1，y+1，z+1)|(i+j+k＝±1∩(i＝0∪j＝0∪k＝0)) (1)

该方程式的含义为：待判断的目标网格与其相邻的六个方向上的六个网格的值的差值的和。可见若目标网格周围的网格为实格则差值为零，否则为1，也就是说该式的值就是目标网格周围六个相邻网格中的空格的数量。对于格网中所有实格以上式进行判断，当上式的值为3、4、5时，目标网格作为种子保留。

步骤2.2.3：采用生长算法对边界种子网格进行生长，得到两组点云数据的所有边界网格；

建立边界种子网格与网格之间的拓扑关系函数和拓扑关系连接数目函数，采用生长算法优先沿边界种子网格的面作为拓扑进行生长，然后依次按照边界种子网格的边、边界种子网格的顶点作为拓扑方向进行生长，得到两组点云数据的所有边界网格。

本实施方式中，建立边界种子网格与网格之间的拓扑关系函数T(x，y，z，i，j，k)如式(2)所示：

T(x，y，z，i，j，k)＝f(x，y，z)-f(x+i，y+j，z+k) (2)

其中，-1≤i≤1，-1≤j≤1，-1≤k≤1，(i，j，k)≠(0，0，0)，f(x，y，z)＝1。

根据公式(2)得出，如果那么得到这样的信息，如果边界种子网格与网格之间的拓扑关系函数T(x，y，z，i，j，k)的函数值为1，那么表明这个实格沿着拓扑方向向量(i，j，k)方向的关系为空，如果边界种子网格与网格之间的拓扑关系函数T(x，y，z，i，j，k)的函数值不等于1，那么实格沿着拓扑方向向量(i，j，k)方向的关就不是空。

建立拓扑关系连接数目函数G(x，y，z)如式(3)所示，利用他来表示拓扑关系连接的数目，G_s(x，y，z)表示面的拓扑关系数，G_e(x，y，z)表示边的拓扑关系数，G_p(x，y，z)表示顶点的拓扑关系数。

G(x，y，z)＝∑_-1≤i≤1∑_-1≤j≤1∑_-1≤k≤1[f(x，y，z)-f(x+1，y+1，z+1)] (3)

利用边界种子网格与网格之间的拓扑关系函数T(x，y，z，i，j，k)和拓扑关系连接数目函数G(x，y，z)根据下述生长法则实现种子边界网格：优先沿边界种子网格的面作为拓扑进行生长，然后依次按照边界种子网格的边、边界种子网格的顶点作为拓扑方向进行生长。

当G_s(x，y，z)＝6或者G_s(x，y，z)＝5时种子边界网格受两者之间的拓扑关系约束沿某个拓扑方向生长否则不生长，当G_s(x，y，z)的值确定时，根据G_e(x，y，z)、G_p(x，y，z)的取值不同接着进行划分，最终完成边界种子网格的生长得到所有的边界种子网格。

步骤2.3：利用边界网格提取两组点云数据的边界点云特征点集S＝(s₁，s₂，...s_i，...s_I)和T＝(t₁，t₂，...t_j，...t_J)。

步骤2.4：利用多尺度特征法从两组点云数据的边界点云特征点中确定相应的配准点对，得到配准点集P₁＝(p₁，p₂，...p_i，...p_n)、Q₁＝(q₁，q₂，...q_j，...q_m)。

步骤2.5：设定配准点距离阈值d_th和配准点角度阈值θ_th，对配准点集P₁＝(p₁，p₂，...p_i，...p_n)、Q₁＝(q₁，q₂，...q_j，...q_m)中的配准点进行误差剔除。

对于配准点集P₁＝(p₁，p₂，...p_i，...p_n)、Q₁＝(q₁，q₂，...q_j，...q_m)中的配准点对{p_i，q_j}，{p_i+1，q_j+1}，{p_i+2，q_j+2}当||p_ip_i+1-q_jq_j+1||≤d_th、||p_i+1p_i+2-q_j+1q_j+2||≤d_th，且||θ₁-θ₂||≤θ_th，则保留该配准点对，否则，删除其配准点对，得到粗配准的配准点集P′₁＝(p₁，p₂，...p_i′，...p_n′)、Q′₁＝(q₁，q₂，...q_i′，...q_m′)，其中，θ₁为p_ip_i+1与p_i+1p_i+2的夹角，θ₂为q_iq_i+1与q_i+1q_i+2的夹角，如图2所示。

采用四元数法确定得到粗配准的配准点集中的配准点对的坐标转换系数，以其中一组点云数据作为目标点云，对两组点云进行初始配准。

步骤3：采用改进的ICP算法对初始配准的点云数据进行精配准。

步骤3.1：针对粗配准的配准点集P′₁＝(p₁，p₂，...p_i′，...p_n′)中任一数据点p_i′，采用kd-tree算法以最近点原则找到在粗配准的配准点集Q′₁＝(q₁，q₂，...q_j′，...q_m′)中距离数据点p_i′最近的三个数据点

本实施方式中，针对粗配准的配准点集P′₁＝(p₁，p₂，...p_i，...p_n′)中任一数据点p_i′，采用kd-tree算法以最近点原则找到在粗配准的配准点集Q′₁＝(q₁，q₂，...q_j′，...q_m′)中距离数据点p_i′最近的三个数据点

步骤3.2：步骤3.2：将数据点构成三角形，以数据点p_i′向所构成三角形的平面作垂线，得到垂足q_j′。

本实施方式中，将数据点构成三角形，以数据点p_i′向所构成三角形的平面作垂线，得到垂足q_j′(x_j′，y_j′，z_j′)。

步骤3.3：若构成的三角形为非钝角三角形，则垂足q_j′(x_j′，y_j′，z_j′)位于三角形中，保留该配准点对，否则，将顶点与三角形的钝角点组成配准点对。

本实施方式中，从任一数据点p_i′点向三角形的平面作垂线，垂足即为想要得到的对应点，锥形形状如图3所示，任一数据点p_i′即图中A点。

当六个角皆为非钝角θ≤90°时，存在下面的几种情况：全为锐角、存在直角、底面三角形为锐角三角形、底面三角形为钝角三角形。如图4所示。可见对于全是锐角三角形的情况来说显然垂足落在三角形内如图4(b)，对于存在直角的情况这时垂足即为三点中的一个点可以认为也在三角形的范围内如图4(a)。

对于底面三角形为钝角三角形的情况如图4(c)比较复杂，经验证若其上的六个角全为非钝角时其垂足也应该落在底面三角形的范围之中。这时垂足落在三维模型的范围之内，是可靠点，可以利用。这时利用公式求出p_i′到三角形的垂足q_j′(x_j′，y_j′，z_j′)，并由p_i′q_j′构成一组最近点对。

当六个角中存在钝角θ＞90°时，锥形模型应该如图5所示。这时得到的垂足落在底面三角形的外面，是虚假点，不能用垂足来进行配对。对于这种情况在比较了点到底面三角形的重心的距离，与顶点到底面各点的距离之后得出这样的结论：顶点到形成钝角的底角点的距离最近。因此用顶点与相应的底角点形成点对，简单快速而且避免了由于虚假点的错误配对。

步骤3.4：重复步骤3.1-步骤3.3，直至得到精配准的配准点集。

步骤4：采用CPC三约束法剔除精配准的配准点集中的误配准点对，得到剔除误差的精配准的配准点对。

所述CPC三约束法包括：方向约束剔除错误点对，刚性约束剔除错误点对，配准误差剔除错误点对。

本实施方式中，精配准的配准点集P″₁＝(p₁，p₂，...p_i″，...p_n″)、Q″₁＝(q₁，q₂，...q_j″，...q_n″)，根据传统CPC理论，对于p_i″在Q″₁＝(q₁，q₂，...q_j″，...q_n")中寻找对应点的原则为q_z″＝argmin||q_j″-p_i″||，得到配准点对(p_i″，q_z″)。

方向约束公式如式(4)所示：

其中，若满足方向约束，则为正确的精配准点对，否则，为错误配准点对，进行删除。

本实施方式中，对于配准点对(p_i″，p_z″)，刚性约束公式如式(5)所示：

||q_l″-q_s″||≤min(||q_z″-p_i″||+||p_l″-p_i″||+||q_l″-p_z″||+||q_s″-p_s″||)(l″，z″∈[2，n″]) (5)

若满足刚性约束，则为正确的精配准点对，否则，为错误配准点对，进行删除。

本实施方式中，对于配准点对(p_i″，p_z″)，(p_a″，q_b″)，定义p_i″点的配准误差为e_i″＝||q_z″-p_i″||，则配准误差剔除公式如式(6)所示：

||q_z″-p_i″+p_i″-p_a″||≤||q_z″-p_i″||+||p_a″-p_i″||＝e_i″+||p_a″-p_i″||(a″，b″∈[2，n″]) (6)

配准点对(p_i″，p_z″)，(p_a″，q_b″)满足公式(6)中的约束条件，则为正确的精配准点对，否则，为错误配准点对，进行删除。

步骤5：根据求得的剔除误差的精配准的配准点对求解转换矩阵，对两组点云进行精配准，得到配准结果。