一种利用KD树搜索的并行化点云配准的方法

一种利用kd树搜索的并行化点云配准的方法
技术领域
1.本发明涉及土木工程建设技术领域，尤其涉及一种利用kd树搜索的并行化点云配准的方法。


背景技术：

2.基础设施是国家和人民生产生活的重要保障，如道路、桥梁、隧道等。然而，如果这些基础设施没有得到及时有效的管理，可能会出现安全事故，对人民的生命财产造成严重威胁。因此，基础设施的建设和维护需要耗费大量的人力、物力、财力等资源。对这些资源的合理利用和管理，可以降低成本，提高效率，为经济发展提供更好的支撑。传统的基础设施管理方法包括目视检查和模型分析，虽然这些方法各有优缺点，但总体来说，它们都存在着主观性强、无法全面检测、需要专业技能等问题。为了提高基础设施管理的精度和效率，现代技术如点云配准、机器学习等成为了必不可少的工具。点云配准在基础设施管理中发挥着重要作用。例如，在道路维护中，可以使用激光扫描仪对道路不同部位进行测量，然后使用点云配准技术将这些测量数据整合起来，以检查道路的状况和需要维护的部位。icp算法作为最经典的点云配准算法，在满足一定条件下可以实现高精度的配准。但算法时间复杂度随着点云点数的增加成指数级增长，目前在海量点云的配准需要耗费大量的时间。综上所述，基础设施管理是一项重要的任务，需要借助现代技术不断提高管理精度和效率。点云配准、机器学习等技术的应用，将为基础设施管理提供更加准确、全面的数据支持，为人们的生产和生活提供更好的服务。
3.icp算法是指，给定源点云p、目标点云q；pi是p中的点，寻找其在点云q上的对应点qi，计算旋转矩阵r与平移矩阵t，使对应点对的欧式距离最小，欧氏距离dist如(1.1)所示。
[0004][0005]
r与t需要满足公式(1.2)所示的f(r,t)值最小，并且对这一系列步骤执行迭代操作，直到满足预先设置的停止迭代条件。
[0006][0007]
公式(1.2)中，f(r,t)为目标函数，表示源点云p旋转平移之后的集合里每一个点与目标点云q中的点qi的欧氏距离的平方和，n
p
为两点云对应的数量。一般会设定参数e与n，e为配准误差阈值，n为最大迭代次数，配准时当误差小于e或迭代次数达到n，则停止配准。具体流程为：
[0008]
(1)在源点云p中取点集pi∈p；
[0009]
(2)目标点云q中取点集qi∈q，其两点关系满足p
i-q
i2
＝min；
[0010]
(3)通过最近点之间的关系计算旋转矩阵r与平移矩阵t，满足
[0011]
(4)对pi使用满足步骤(3)中条件的变换矩阵，使源点云得到新的位置；
[0012]
(5)计算配准误差；
[0013]
(6)配准时当误差小于e或迭代次数达到n，则停止配准，输出最终的变换矩阵，否则返回步骤(2)，直到满足给定条件为止。
[0014]
这样经过每次迭代，点云p与点云q中的对应点会越来越趋近于重合，最终得到完整的点云模型。这就是icp的基本思想，但是传统的icp算法遍历点云数据需要逐点遍历，这种遍历方式效率低下。
[0015]
同时，近年来，随着gpu的发展，gpu计算能力得到了极大提升，不仅能处理传统的图形计算，对数据的并行计算上也得到了广泛的应用。nvidia公司的统一计算设备架构(cuda)可以有效解决一些复杂的计算。
[0016]
在cuda的架构下，可以将程序划分成两个部分，分别是对cuda程序进行控制和管理的主机(host)端和负责执行cuda核函数的设备(device)端。
[0017]
线程(thread)是计算芯片在cuda架构下的最小单元。一个线程运行一个特定的代码段，可以访问其自己的寄存器和本地内存。块(block)是一个线程组，由多个线程组合而成，一个块内的所有线程共享同一块内存，并且可以通过同步机制进行协作。网格(grid)是一个线程块组成的二维网格。一个网格由多个块组成，不同块之间可以相互通信。不同的网格则可以执行不同的程序(即kernel)。线程-块-网格的结构如图6所示。
[0018]
且并行程序流程为：host端首先读取数据，然后在gpu中开辟所需的内存空间，再复制到gpu的内存中，再由gpu执行device端程序，执行完成后将运行结果传回host端，最后释放gpu所开辟的内存。
[0019]
本方案基于上述方法，将其应用于点云配准中，来解决现阶段传统的icp算法在点云配准工作中效率低下的问题。


技术实现要素：

[0020]
为解决传统的icp算法遍历点云数据需要逐点遍历，遍历方式效率低下的技术问题，本发明提供一种利用kd树搜索的并行化点云配准的方法。
[0021]
本发明采用以下技术方案实现：一种利用kd树搜索的并行化点云配准的方法，包括如下步骤：
[0022]
步骤一、首先在cpu端读取点云数据，构造kd树建树函数；
[0023]
步骤二、在gpu端开辟点云及kd树所需要的内存空间，将数据由cpu传入gpu；
[0024]
步骤三、并且在gpu端利用kd树并行寻找两幅点云的对应点；
[0025]
步骤四、通过svd法计算旋转平移矩阵，对源点云进行变换，满足最大迭代次数或满足距离阈值时完成迭代，否则返回到步骤三，最后将配准后的点云数据由gpu传回cpu，并释放gpu开辟的内存空间。
[0026]
在所述步骤1中：
[0027]
首先在cpu端读取点云数据，构造kd树建树函数，使用c++语言，编写代码，接受一个存储点集的向量、点集的起始位置和结束位置、当前节点的索引，以及存储kd树节点的向量和当前深度作为参数，采用递归的方式，将点集分割成两个子集，每个子集都是一个更小的kd树；
[0028]
该代码首先根据当前深度的维度，对点集进行排序，然后选择中间点作为父节点，并将其添加到kd树中，接着，递归地对左子树和右子树进行相同的操作，直到所有的点都被添加到kd树中为止。
[0029]
利用kd树进行最近邻查找遵循以下步骤：假设查询点为q，首先从树的根节点开始，计算并记录点q和树上每个节点之间的距离，然后沿着树往下递归地进行对比，直到叶子节点；
[0030]
在上述过程中，需要比较查询点q在k维上的值与当前节点对应维度的值m，并且用q(k)表示查询点q在k维上的值，如果q(k)＜m，则进入左子树，否则进入右子树；
[0031]
当查询点q到达叶子节点时，需要计算q和叶子节点之间的距离，然后，记录距离查询点q最近的节点为当前的最近邻点，记为p
cur
，并记录最小距离为d
cur
。
[0032]
利用kd树进行最近邻查找还包括回溯操作，目的在于：未进行距离比较的节点中找到是否存在相对于当前最近邻点距查询点q更近的节点；
[0033]
具体地，寻找可能存在距离点q和其父节点更近的未比较过距离的分支节点，其与点q的距离可能小于给定距离d
cur
，并将其标记为可能更小的分支。以判断此分支中是否存在比当前最近邻点更近的点p0，在进入该分支节点进行比较时，如果存在更近的节点，则更新当前最近邻点为p
cur
，并将距离更新为d
cur
。若点q和父节点下未比较过的分支之间的距离大于d
cur
，则可以认为该分支中不存在比点q更近的点。整个回溯操作是从下往上进行的，直到回溯到根节点或者没有与点p更近的分支为止。
[0034]
所述步骤4中，通过奇异值分解(svd)法计算旋转平移矩阵，svd法是点云配准过程中求解矩阵常用的方法，首先计算源点云p、目标点云q的质心m
p
、mq与协方差矩阵σ(p,q)，见公式(1.3)、(1.4)、(1.5)；
[0035][0036][0037][0038]
求得协方差矩阵后，对其进行奇异值分解e＝uλv
t
。其中，u、v为3*3的正交矩阵，λ由协方差矩阵特征值构成的对角矩阵，则旋转矩阵r和平移矩阵t可表示为如(1.6)、(1.7)所示：
[0039]
r＝uv
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.6)
[0040]
t＝m
p-rmqꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.7)
[0041]
对源点云进行变换，r*p+t代表从源点云旋转平移的变换过程，在转换过程中需要使用三维射影变换矩阵h来完成，如公式(1.8)：
[0042]
[0043]
h可简化为为旋转矩阵，t＝[t
x t
y tz]
t
为旋转矩阵，v＝[v
x v
y vz]为透视变量，s为缩放因子，本专利研究无缩放刚性点云，故透视变换为0，缩放因子s＝1，则变换矩阵h为
[0044]
令点云数据沿x、y、z轴旋转的角度分别为α、β、γ，则旋转矩阵还可以表示为公式(1.9)的形式
[0045][0046]
满足最大迭代次数或满足距离阈值时完成迭代，否则返回到寻找对应点的步骤，最后将配准后的点云数据由gpu传回cpu，并释放gpu开辟的内存空间。
[0047]
相比现有技术，本发明的有益效果在于：
[0048]
本发明基于gpu以及cuda实现的利用kd树搜索的点云icp精配准与传统icp算法有着相同的精度，并且能够节约大量时间，提升配准效率。
[0049]
本发明通过使用并行化点云配准流程，可以在海量点云数据配准中节省大量的时间，为后续的点云处理工作提供了基础。这一流程的实现基于gpu和cuda，能够充分发挥图形处理器的计算能力，为点云配准提供了更加高效、准确的解决方案，为相关领域的研究和应用提供有力支持。
[0050]
相较于传统的icp算法需要遍历两幅点云内所有点，此过程是传统icp算法中耗时最长的，仅使用cpu对大数据量点云进行点的遍历效率太低。所以调用gpu的多个计算核心同时进行计算可以大幅缩减计算耗时，本发明采用gpu可以加速icp配准的过程。
附图说明
[0051]
图1为本发明使用的kd树结构图；
[0052]
图2为配准前的点云示意图，蓝色为目标点云，红色为源点云；
[0053]
图3为配准后的点云示意图，红色为配准后的点云，蓝色为目标点云；
[0054]
图4为不同点云数量时的配准耗时示意图；
[0055]
图5为本方案的流程图；
[0056]
图6为线程-块-网格结构图。
具体实施方式
[0057]
下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。
[0058]
实施例1：
[0059]
一种利用kd树搜索的并行化点云配准的方法，包括如下步骤：
[0060]
步骤一、首先在cpu端读取点云数据，构造kd树建树函数；
[0061]
步骤二、在gpu端开辟点云及kd树所需要的内存空间，将数据由cpu传入gpu；
[0062]
步骤三、并且在gpu端利用kd树并行寻找两幅点云的对应点；
[0063]
步骤四、通过svd法计算旋转平移矩阵，对源点云进行变换，满足最大迭代次数或满足距离阈值时完成迭代，否则返回到步骤三，最后将配准后的点云数据由gpu传回cpu，并释放gpu开辟的内存空间。
[0064]
在所述步骤1中：
[0065]
首先在cpu端读取点云数据，构造kd树建树函数，使用c++语言，编写代码，接受一个存储点集的向量、点集的起始位置和结束位置、当前节点的索引，以及存储kd树节点的向量和当前深度作为参数，采用递归的方式，将点集分割成两个子集，每个子集都是一个更小的kd树；
[0066]
该代码首先根据当前深度的维度，对点集进行排序，然后选择中间点作为父节点，并将其添加到kd树中，接着，递归地对左子树和右子树进行相同的操作，直到所有的点都被添加到kd树中为止。
[0067]
利用kd树进行最近邻查找遵循以下步骤：假设查询点为q，首先从树的根节点开始，计算并记录点q和树上每个节点之间的距离，然后沿着树往下递归地进行对比，直到叶子节点；
[0068]
在上述过程中，需要比较查询点q在k维上的值与当前节点对应维度的值m，并且用q(k)表示查询点q在k维上的值，如果q(k)＜m，则进入左子树，否则进入右子树；
[0069]
当查询点q到达叶子节点时，需要计算q和叶子节点之间的距离，然后，记录距离查询点q最近的节点为当前的最近邻点，记为p
cur
，并记录最小距离为d
cur
。
[0070]
利用kd树进行最近邻查找还包括回溯操作，目的在于：未进行距离比较的节点中找到是否存在相对于当前最近邻点距查询点q更近的节点；
[0071]
具体地，查找与点q和父节点下未比较过距离的分支节点之间的距离可能小于d
cur
的分支，以判断此分支中是否存在比当前最近邻点更近的点p0，在进入该分支节点进行比较时，如果存在更近的节点，则更新当前最近邻点为p
cur
，并将距离更新为d
cur
。若点q和父节点下未比较过的分支之间的距离大于d
cur
，则可以认为该分支中不存在比点q更近的点。整个回溯操作是从下往上进行的，直到回溯到根节点或者没有与点p更近的分支为止。
[0072]
所述步骤4中，通过奇异值分解(svd)法计算旋转平移矩阵，svd法是点云配准过程中求解矩阵常用的方法，首先计算源点云p、目标点云q的质心m
p
、mq与协方差矩阵σ(p,q)，见公式(1.3)、(1.4)、(1.5)；
[0073][0074][0075][0076]
求得协方差矩阵后，对其进行奇异值分解e＝uλv
t
。其中，u、v为3*3的正交矩阵，λ由协方差矩阵特征值构成的对角矩阵，则旋转矩阵r和平移矩阵t可表示为如(1.6)、
(1.7)所示：
[0077]
r＝uv
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.6)
[0078]
t＝m
p-rmqꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.7)
[0079]
对源点云进行变换，r*p+t代表从源点云旋转平移的变换过程，在转换过程中需要使用三维射影变换矩阵h来完成，如公式(1.8)：
[0080][0081]
h可简化为为旋转矩阵，t＝[t
x t
y tz]
t
为旋转矩阵，v＝[v
x v
y vz]为透视变量，s为缩放因子，本专利研究无缩放刚性点云，故透视变换为0，缩放因子s＝1，则变换矩阵h为
[0082]
令点云数据沿x、y、z轴旋转的角度分别为α、β、γ，则旋转矩阵还可以表示为公式(1.9)的形式
[0083][0084]
满足最大迭代次数或满足距离阈值时完成迭代，否则返回到寻找对应点的步骤，最后将配准后的点云数据由gpu传回cpu，并释放gpu开辟的内存空间。
[0085]
本方案以图2、图3中的点云数据做出实验。
[0086]
图中：(a)飞机；(b)犰狳；(c)古建筑；(d)部件；(e)亚洲龙；(f)道路。
[0087]
图2为原始点云数据，图2中红色点云为源点云，蓝色点云为目标点云；
[0088]
经本方案配准后的点云如图3所示，图3中红色点云为配准后的点云，蓝色点云为目标点云。由图3可以看出经过精配准后，两幅点云中的对应点明显趋于重合。
[0089]
并且经研究发现，点云配准数据以及分析结果如表1所示。
[0090][0091]
并且icp配准耗时与本文改进算法的配准耗时如图4所示。由图4可以看出，点数量较少时，加速效果不明显，随着点云数量的增加，本文改进算法的加速越明显。
[0092]
上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。

技术特征：
1.一种利用kd树搜索的并行化点云配准的方法，其特征在于，包括如下步骤：步骤一、首先在cpu端读取点云数据，构造kd树建树函数；步骤二、在gpu端开辟点云及kd树所需要的内存空间，将数据由cpu传入gpu；步骤三、并且在gpu端利用kd树并行寻找两幅点云的对应点；步骤四、通过svd法计算旋转平移矩阵，对源点云进行变换，满足最大迭代次数或满足距离阈值时完成迭代，否则返回到步骤三，最后将配准后的点云数据由gpu传回cpu，并释放gpu开辟的内存空间。2.如权利要求1所述的一种利用kd树搜索的并行化点云配准的方法，其特征在于，在所述步骤1中：首先在cpu端读取点云数据，构造kd树建树函数，使用c++语言，编写代码，接受一个存储点集的向量、点集的起始位置和结束位置、当前节点的索引，以及存储kd树节点的向量和当前深度作为参数，采用递归的方式，将点集分割成两个子集，每个子集都是一个更小的kd树；该代码首先根据当前深度的维度，对点集进行排序，然后选择中间点作为父节点，并将其添加到kd树中，接着，递归地对左子树和右子树进行相同的操作，直到所有的点都被添加到kd树中为止。3.如权利要求1所述的一种利用kd树搜索的并行化点云配准的方法，其特征在于，利用kd树进行最近邻查找遵循以下步骤：假设查询点为q，首先从树的根节点开始，计算并记录点q和树上每个节点之间的距离，然后沿着树往下递归地进行对比，直到叶子节点；在上述过程中，需要比较查询点q在k维上的值与当前节点对应维度的值m，并且用q(k)表示查询点q在k维上的值，如果q(k)＜m，则进入左子树，否则进入右子树；当查询点q到达叶子节点时，需要计算q和叶子节点之间的距离，然后，记录距离查询点q最近的节点为当前的最近邻点，记为p
cur
，并记录最小距离为d
cur
。4.如权利要求3所述的一种利用kd树搜索的并行化点云配准的方法，其特征在于，利用kd树进行最近邻查找还包括回溯操作，目的在于：未进行距离比较的节点中找到是否存在相对于当前最近邻点距查询点q更近的节点；具体地，查找与点q和父节点下未比较过距离的分支节点之间的距离可能小于d
cur
的分支，以判断此分支中是否存在比当前最近邻点更近的点p0，在进入该分支节点进行比较时，如果存在更近的节点，则更新当前最近邻点为p
cur
，并将距离更新为d
cur
。若点q和父节点下未比较过的分支之间的距离大于d
cur
，则可以认为该分支中不存在比点q更近的点。整个回溯操作是从下往上进行的，直到回溯到根节点或者没有与点p更近的分支为止。5.如权利要求1所述的一种利用kd树搜索的并行化点云配准的方法，其特征在于，所述步骤4中，通过奇异值分解(svd)法计算旋转平移矩阵，svd法是点云配准过程中求解矩阵常用的方法，首先计算源点云p、目标点云q的质心m
p
、m
q
与协方差矩阵σ(p,q)，见公式(1.3)、(1.4)、(1.5)；
求得协方差矩阵后，对其进行奇异值分解e＝uλv
t
。其中，u、v为3*3的正交矩阵，λ由协方差矩阵特征值构成的对角矩阵，则旋转矩阵r和平移矩阵t可表示为如(1.6)、(1.7)所示：r＝uv
t
ꢀꢀꢀꢀ
(1.6)t＝m
p-rm
q
ꢀꢀꢀꢀ
(1.7)对源点云进行变换，r*p+t代表从源点云旋转平移的变换过程，在转换过程中需要使用三维射影变换矩阵h来完成，如公式(1.8)：h可简化为h可简化为为旋转矩阵，t＝[t
x t
y t
z
]
t
为旋转矩阵，v＝[v
x v
y v
z
]为透视变量，s为缩放因子，本专利研究无缩放刚性点云，故透视变换为0，缩放因子s＝1，则变换矩阵h为令点云数据沿x、y、z轴旋转的角度分别为α、β、γ，则旋转矩阵还可以表示为公式(1.9)的形式满足最大迭代次数或满足距离阈值时完成迭代，否则返回到寻找对应点的步骤。

技术总结
本发明涉及土木工程建设技术领域，公开了一种利用KD树搜索的并行化点云配准的方法，包括如下步骤：步骤一、首先在CPU端读取点云数据，构造KD树建树函数；步骤二、在GPU端开辟点云所需要的内存空间，将数据由CPU传入GPU；步骤三、并且在GPU端利用KD树并行寻找两幅点云的对应点；步骤四、通过SVD法计算旋转平移矩阵，对源点云进行变换，满足最大迭代次数或满足距离阈值时完成迭代，否则返回到步骤三。本发明可以在海量点云数据配准中节省大量的时间，为后续的点云处理工作提供了基础。这一流程的实现基于GPU和CUDA，能够充分发挥图形处理器的计算能力，为点云配准提供了更加高效、准确的解决方案，为相关领域的研究和应用提供有力支持。有力支持。有力支持。


技术研发人员：卞开春 李明 王嘉琛 吴言安 袁斌 叶周润 张树峰 崔浩 谢荣晖 李静
受保护的技术使用者：合肥工业大学 中煤第三建设（集团）有限责任公司
技术研发日：2023.06.26
技术公布日：2023/9/20