基于k-d tree的ICP三维点云拼接方法

李艳，屈仁飞，顾菘，谢燕梅

(1.成都航空职业技术学院无人机产业学院，四川成都 610199；2.成都西南交大研究院有限公司，四川成都 610031)

1 概述

三维点云在逆向工程[1]、文物保护[2]、口腔医学[3]与无人驾驶[4]等行业中得到了广泛的应用。在获取三维点云时，受被测物体的几何形貌、测量设备等因素的限制，测量设备需要从多个角度对被测物体进行数据采集，采集得到的各片点云均位于设备坐标系下。因此，需要计算出旋转平移矩阵，调整多视角测量的各片三维点云在空间中的位姿，并统一到同一坐标系当中，从而获得完整的被测物体三维点云。计算旋转平移矩阵的过程，即是求解三维点云拼接问题的过程。

为解决三维点云拼接问题，国内外学者提出了众多拼接算法[5-8]，一般可将拼接过程分解为粗拼接与精拼接两个步骤。基于主成分分析[5](Principal Component Analysis，PCA)的拼接算法是通过计算两片点云的主成分与质心坐标值，然后，计算得到点云之间的旋转平移矩阵，实现拼接，但是当点云主成分不明确时，难以实现拼接，且易出现主轴反向问题[9]。采样一致性初始配准法[6](Sample Consensus Initial Aligment，SAC-IA)使用点云法线、快速点特征直方图[10](Fast Point Feature Histograms，FPFH)等特征信息计算两片点云之间的对应关系进行拼接，但其效率低、拼接精度较差。正态分布变换[7](Normal Distributions Transform，NDT)则是通过标准最优化技术确定两片点云之间的最优对应关系，并进行拼接，但该算法的收敛域相对较差、对点云初始空间位姿有一定要求。上述算法均适用于粗拼接，但难以满足精度要求，具有一定局限性，故三维点云拼接需在粗拼接的基础上进行精拼接。

目前最经典的精拼接算法是迭代最近点[8](Iterative Closest Point，ICP)算法，其采用最小二乘估计计算点集之间的关系，通过迭代计算使最近点的误差之和最小。两片点云中的拼接点集决定了ICP 算法的收敛速度与拼接精度，若点对匹配错误，则会导致算法陷入局部最优解。

为此，本文提出了一种基于k-d tree 的ICP 三维点云拼接算法，确立了点云拼接的对应点集，满足高精度的三维拼接要求。

2 三维点云k-d tree

k-d树是一种能够存储和检索k维数据的数据结构。它的根节点在di(i=1～k)维上形成一个超平面，将k维空间分成两个子空间。在di维中小于超平面值的数据放在左子树中，大于超平面值的数据放在右子树中。

2.1 建立三维点云k-d tree

为达到在三维点云中快速检索近邻点的目的，需要对三维点云建立相应的k-d tree，建立流程如图1所示。

图1 三维点云k-d tree建立流程

首先，计算三维点云在X、Y、Z三个维度上的方差，按照方差大小轮流选取分割维度，在分割维度上选取中值作为树的根节点，则中值所在处形成的超平面将空间分割左右子空间，如此反复直至子空间中无数据。

如图2所示为一组三维点通过上述方法建立的k-d tree，图中△数据位于分割维度。

图2 三维数据生成的k-d tree

2.2 最近邻搜索

完成三维点云k-d tree的建立之后，便可在k-d tree中对目标点进行检索，搜寻k-d tree中距离目标点最近的点，即是k-d tree的最近邻搜索，其过程如下：

(1)从根节点开始，递归地往子节点搜索。搜索方向的决定方法与插入元素的方法相同，即如果目标点在超平面的左边则进入左子节点，在右边则进入右子节点。

(2)一旦搜索到叶节点，该叶节点就被视为当前最佳点，计算该叶节点与目标点之间的距离。

(3)解开递归，并对每个经过的节点进行下列步骤：

①如果当前点比当前最佳点更接近目标点，则将当前最佳点更新为当前点；

②检查另一子树是否存在更近的点，若存在，则从该节点往下搜索。

(4)当根节点搜索完毕后，便完成了最近邻搜索。

3 ICP算法

ICP 算法的基本原理是：在目标点云P中选取点pi，在待匹配的源点云Q中搜索最近邻点qi，并计算出pi与qi的最优匹配旋转矩阵R和平移向量T，从而使如式(1)所示的误差函数最小化。

式(1)中pi为目标点云P中的点，qi为源点云Q中与pi对应的最近点，n为最近邻点的对数，R为旋转矩阵，t为平移向量。

3.1 传统的ICP算法

传统ICP算法的步骤如下：

(1)取目标点云P中的点集pi∈P；

(2)在源点云Q中，找到pi的对应点集qi∈Q，使得‖qi-pi‖=min；

(3)基于对应点对，计算出相应的旋转平移矩阵[R|t]；

(4)利用步骤(3)得到的旋转平移矩阵[R|t]对pi进行旋转平移变换，得到新的对应点集pi'=Rpi+t,pi∈P；

(5)计算pi'和对应点集qi之间的平均距离d，如式(2)所示；

(6)给定一个距离阈值ε，并预设迭代计算的最大迭代次数；

(7)当式(2)中的d小于给定的距离阈值ε，或者迭代计算的次数大于预设的最大迭代次数时，则停止进行迭代计算。否则，返回步骤(2)继续计算，直到满足收敛条件。

3.2 基于k-d tree的ICP算法

ICP 算法中的旋转平移矩阵采用最小二乘估计计算，原理简单，精度高。然而，由于采用迭代的方法进行计算，该算法效率低。再者，ICP 算法对初始位姿有着较高要求，若拼接时，目标点云与源点云存在大量非对应点集，将导致算法陷入局部最优解。鉴于此，使用基于k-d tree的ICP算法，对拼接的目标点云与源点云进行优化，使得拼接的对应点集更加精准，从而提升拼接效率，避免算法陷入局部最优。

图3 为基于k-d tree 的ICP 算法流程，在迭代计算之前，使用k-d tree 对目标点云中的对应点集进行初步选取，避免过多的非对应点集参与到点云拼接中，从而避免算法陷入局部最优解。同时，在迭代计算中，使用k-d tree 快速搜索最近点，获取对应点集，提高点云的拼接效率。

图3 基于k-d tree的ICP算法流程

4 试验与结果分析

为了验证上述算法对三维点云拼接的正确性与有效性，使用无人机搭载激光雷达对现场房屋进行扫描试验，如图4 所示。扫描得到的部分三维激光点云数据如图5所示，可见点云在三维空间中存在明显错位。

图4 现场试验

图5 现场房屋点云

设图5 中左侧(绿色)点云为目标点云，右侧(红色)点云为待匹配的源点云，使用两种不同的算法分别对目标点云和源点云进行拼接：(a)传统的ICP算法；(b)本文提出的基于k-d tree的ICP拼接算法。

在处理器为Intel(R) Core(TM) i7-8650U CPU @ 1.90GHz 2.11 GHz，RAM 为16.0G 的计算机上，在Windows 10 系统下编写C++程序进行拼接测试，算法(a)的拼接时间为128.879s，算法(b)的拼接时间为3.462s，拼接效果如图6所示。

图6 拼接效果

传统的ICP算法计算出的旋转平移矩阵[R|t]a如式(3)所示，该矩阵使得源点云中的地面点云与目标点云中的地面点云之间形成了一定夹角，算法陷入了局部最优，无法达到所需的拼接目的；本文提出的基于k-d tree 的ICP 拼接算法计算出的旋转平移矩阵[R|t]b如式(4)所示，该矩阵使得源点云在屋顶处与目标点云实现了较好的重合拼接。

从图5中可以看出，源点云只需通过一定的平移便可使两点云拼接在一起，即算法计算出的旋转矩阵R越接近单位矩阵，该算法的准确度越高、有效性越好。对比式(3)和式(4)，可见式(4)相对于式(3)更接近单位矩阵，即本文提出的基于k-d tree的ICP拼接算法对传统的ICP算法有着明显的改善。

5 结论

1)建立了三维点云k-d tree，确定了三维点云k-d tree的最近邻搜索方法。在传统ICP算法的基础上，提出了一种基于kd tree的ICP三维点云拼接算法，并且确立了该算法的流程。

2)本文提出的基于k-d tree 的ICP 三维点云拼接算法，可避免ICP算法陷入局部最优，有效改善了点云拼接效果。相对于传统的ICP算法，该算法可有效减少目标点云与源点云中的错误匹配点对，同时提升算法效率。