CN110097581B - 基于点云配准icp算法构建k-d树的方法 - Google Patents

Abstract

本发明提供的基于点云配准ICP算法构建K‑D树的方法，该方法将点云空间划分为多个子空间，定义存在数据点的子空间为有效子空间，针对有效子空间建立K‑D树，以此来降低点云K‑D树的复杂度，加快硬件构建K‑D树的速度，同时减轻硬件的存储负担。该方法用直接存储方式来存储点云数据，根据子空间坐标直接获取该子空间内所有数据的地址，以此利用硬件并行处理的优势加快最近邻查询，该方法为加速迭代最近点算法的硬件系统提供了解决方案。

Description

基于点云配准ICP算法构建K-D树的方法

技术领域

本发明涉及集成电路设计领域，具体为一种基于点云配准ICP算法构建K-D树的方法。

背景技术

点云是三维空间中一系列点的集合，利用特定设备扫描某物体表面可得到表示该物体表面特征的点云数据。点云中任意一点通常以三维直角坐标系的坐标来表示。基于对点云数据的识别、过滤、配准等操作以完成还原物体三维模型的过程称为三维重建。点云配准是影响三维重建性能的关键步骤。

点云配准是实现将两片点云拼接到同一坐标系的技术，分为粗配准和精配准两步。点云精配准是点云配准中最耗时的环节，也是导致点云配准无法满足实时性场景速度需求的主要因素。由Besl和Mckay于1992年提出的ICP(Iterative Closest Point)算法，是目前实现点云精配准使用最广泛的算法。在ICP算法中，定义一片点云为目标点云T，另一片点云为模型点云M，算法实现将模型点云转换到目标点云所在的坐标系。

ICP算法中查找最近点对的过程是整个ICP算法的速度瓶颈。目前最主流的ICP算法是将目标点云T用K-D树的形式组织，利用K-D树的空间分割特性来加速查找。该方法在软件上已被证明性能极佳，但软件实现受串行处理限制仍无法满足实时场景的速度要求。采用硬件加速ICP算法成为必然，但在硬件上构建K-D树还存在两个问题：

1.现有技术中构建的K-D树，是将目标点云中所有点作为K-D树的节点。其中每输出一个节点都要进行一次排序，且所有节点串行输出。对于数量庞大的点云来说，该过程耗时非常长，且无法利用硬件并行优势来加速；

2.现有技术中采用链表形式来存储K-D树节点，每个节点除了存储该节点的数据以外，还要存储该节点的父子节点的地址，这种间接存储的方式不仅加重了硬件存储的负担，还大大限制了硬件读取数据的速度。

发明内容

针对现有技术中存在的问题，本发明提供一种基于点云配准ICP算法构建K-D树的硬件设计方法，该方法降低了点云K-D树的复杂度，减少了构建K-D树的数据量，提高了硬件构建K-D树的速度，同时减轻了硬件的存储负担，为硬件实现加速迭代最近点算法提供了解决方案。

本发明是通过以下技术方案来实现：

基于点云配准ICP算法构建K-D树的方法，包括以下步骤：

步骤1、根据点云数据的存储位宽，将点云空间划分为多个子空间并定义子空间坐标；

步骤2、为每个子空间分配地址连续的存储空间，以及1bit的有效标志位；

步骤3、读取点云数据，确定每个数据所属的子空间坐标，将该数据存储在该子空间对应的存储空间，同时将该子空间的有效标志位置为1，该子空间为有效子空间；

步骤4、根据有效子空间建立K-D树，K-D树上的叶子节点均为有效子空间的坐标，非叶子节点均为分割面方向划分有效子空间的分割值；

步骤5、存储K-D树信息，每个非叶子节点存储该节点的分割值，并对分割值增加两个比特标志位，分别表示该节点的左右子节点是否为叶节点；每个叶子节点存储该节点值，即子空间的坐标值。

优选的，步骤1中划分多个子空间的方法如下：

获取点云数据在x、y、z三个维度上的存储位宽N_x、N_y、N_z，并根据存储位宽确定每个维度的编码位数n_x、n_y、n_z，根据编码位数n_x、n_y、n_z的二进制码对点云空间的x、y、z维度编码，使x、y、z三个维度分别分成

组，将整个点云空间划分为

个子空间，每个子空间的坐标为{S_x,S_y,S_z}；

其中，满足条件，n_x<N_x，n_y<N_y，n_z<N_z；

S_x为子空间在x维度的坐标，S_y为子空间在y维度的坐标，S_z为子空间在z维度的坐标。

优选的，将存储位宽N_x、N_y和N_z按相同比例缩小后取整得到每个维度的编码位数n_x、n_y和n_z。

优选的，步骤2中分配地址连续的存储空间的方法如下：

将目标点云的存储空间的地址分为

个区间，使得一个子空间对应一个地址连续的存储区间，记录每个子空间所占用存储空间的起始地址，子空间占用存储的地址范围如下：

S*2^k～(S+1)*2^k-1

其中，S为子空间的编码，k＝N_x+N_y+N_z-n_x-n_y-n_z。

优选的，步骤4中K-D树的建立方法包括以下步骤：

S1、定义所有有效子空间的坐标为一个数据域，从第一层开始，将数据域的子空间坐标在split方向排序，找到中位值，该中位值即为根节点，小于中位值的子空间归为根节点的左数据域，大于等于中位值的子空间归为右数据域；

S2、左数据域和右数据域分别在下一层的split方向排序取中位值，产生当前节点的左子节点和右子节点，重复该步骤，逐层产生分割节点；

S3、当数据域的子空间数目为1时，将该子空间的坐标作为叶子节点输出，当所有子空间的坐标输出后，K-D树建立完成。

优选的，步骤5中存储K-D树信息的方法如下：

令根节点的存储地址为1，任意一节点的左子节点的存储地址为其父节点的地址左移一位，右子节点的存储地址为其父节点的地址左移一位后加1，得到所有节点的存储地址；

每个非叶子节点用n_x+n_y+n_z+2bit存储，其中最高位和次高位比特分别表示该节点的左右子节点是否为叶子节点，其余比特表示该节点的分割值；

每个叶子节点用n_x+n_y+n_z比特来存储，节点值表示有效子空间的坐标。

与现有技术相比，本发明具有以下有益的技术效果：

本发明提供的基于点云配准ICP算法构建K-D树的方法，首先对点云空间进行了分割，并为分割出的子空间分配了地址连续的存储空间。在读入目标点云数据时，确定每个数据点所属的子空间，并将该点存入所属子空间对应的存储空间，通过子空间地址可直接计算出该子空间所以点的存储地址，以此实现了点云数据的直接存储，使得硬件能够并行地读取多个点，解决了现有技术中使用链表存储只能串行读取的问题，有效地提高了硬件读取点云的效率。

同时，定义存在数据点的子空间为有效子空间，构建一棵关联有效子空间的K-D树。本发明通过将对点建树改为对有效子空间建树，将构建K-D树的数据总量从目标点云所有点的数量降低为有效子空间的数量，使得K-D树的节点数量有了数量级上的降低，从而减少了K-D树建树过程中循环的次数，降低了排序时间，显著地提高了K-D树建树的效率。同时，由于K-D树节点数量的降低，本发明也减轻了硬件存储的负担。

利用本发明的K-D树查找近邻时，一次查找可将最近点所在范围锁定为一个或多个子空间，通过子空间的坐标，确定点云数据的存储地址范围，即可利用硬件并行处理的优势，加速查找。

附图说明

图1为本发明方法的流程图；

图2为本发明的子空间坐标定义说明图；

图3为本发明的有效空间标记示意图；

图4为本发明的构建和存储K-D树的示意图；

图5为本发明仿真时采用的点云数据。

具体实施方式

下面结合附图对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

参阅图1，基于点云配准ICP算法构建K-D树的方法，包括以下步骤：

步骤1、获取点云数据在x、y、z三个维度上的存储位宽N_x、N_y、N_z，根据存储位宽确定每个维度的编码位数n_x、n_y、n_z。

只需满足条件：n_x<N_x，n_y<N_y，n_z<N_z，且n_x、n_y、n_z均为整数。

例如可以将N_x、N_y、N_z按相同比例缩小后取整得到n_x、n_y、n_z。

步骤2、组合三个维度的编码位数，将点云空间划分为

个子空间，每个子空间的坐标采用该空间在x方向坐标，y方向坐标和z方向坐标按顺序拼接的形式生成，图2为二维空间划分和子空间坐标定义的示意图。

采用子空间的坐标将空间分割，但每个子空间的坐标值无需在硬件上存储，在硬件上存储n_x、n_y、n_z的值；

步骤3、在硬件上分配一个大小为

的子空间有效表T，并初始化每个比特为0，表中的一个比特对应一个子空间的状态。

若某子空间的状态为0，则代表该子空间无效，不存在点云数据；若为1，代表该子空间有效，即存在点云数据；

步骤4、分配目标点云的存储空间，将该存储空间的地址分为

个区间，使得一个子空间对应一个地址连续的存储区间，记录每个子空间所占用存储空间的起始地址；设某子空间的编码为S，该子空间占用存储的地址范围如下：

S*2^k～(S+1)*2^k-1

其中，k＝N_x+N_y+N_z-n_x-n_y-n_z。在硬件实现上，将S左移k次即可得到该子空间的起始地址；

步骤5、依次读入目标点云数据，确定每一个点对应的子空间坐标S_d，也就是该点落在的子空间，S_d的值可以在读入点云数据时通过拼接运算得到，S_d表达式如下：

S_d＝{d_x,d_y,d_z}，

其中，d_x、d_y、d_z分别是该点x、y、z坐标值的高n_x、n_y、n_z位；将该点存储于S_d对应的存储空间，同时将表T中S_d对应的状态为置为1，表示S_d空间有效，图3为有效空间标记示意图。

步骤6、遍历表T，统计出有效子空间的总数目total，并为每个有效子空间分配存储空间，依次将有效子空间的坐标存入。

步骤7、采用自顶而下的方式对有效子空间构建K-D树，具体在硬件上实现的方法为：

首先，规定点云空间的划分规则；

定义split＝0、split＝1、split＝2分别表示在垂直于x、y、z方向的平面分割，同时定义K-D树的层数和split之间的对应关系；

然后，执行排序找中值操作，用一个堆栈来记录未被排序的子空间编码所在的地址边界，堆栈内的初始数据为{1,total}，从树的第一层开始，从堆栈中pop出一组地址边界，对该地址范围内的子空间坐标在split方向上排序，取中位值输出为该层节点，小于该节点值的子空间编码在下一层的split上重新排序找中值，大于等于节点值的子空间编码的存储地址边界被push到堆栈中。逐层产生节点，直到分支建树结束，即输出叶节点。当一条分支完成后，从堆栈中pop出一组数组边界，开始下一条分支的节点输出。重复分支生成的操作，直到所有叶节点输出，建树完成。

步骤8、依次接收K-D树的节点，完成K-D树的存储。

令根节点的存储地址为1，任意一节点的存储地址为a，该节点的左子节点的存储地址为a<<1，右子节点的存储地址为a<<1+1。根据步骤七中节点生成的方式，可计算出每个节点的存储地址。

图4是对图3中点云有效空间构建和存储K-D树的示意图。K-D树上的每个非叶子节点用n_x+n_y+n_z+2比特来存储，最高比特和次高比特用来标志该节点的左子节点和右子节点是否为叶子节点，该值为1时，表示是叶子节点，为0时表示非叶子节点；叶子节点存储的内容为有效子空间的坐标值。

与现有技术相比，本发明的优点如下：

本发明的方法构建的K-D树，建立的是有效子空间之间的联系，该树上的叶子节点均表示有效子空间的坐标，非叶子节点均为子空间之间的分割值。

设目标点云的数据量为T，目标点云占用的有效子空间数目为V，基于本发明空间分割的策略，可得V<<T。在构件K-D树速度方面，现有构建K-D树的方法需要处理的数据量为T，构建K-D树过程耗时长。本发明提供的方法构件K-D树处理的数据量为V，通过降低K-D树的复杂度，提高了在硬件上构件K-D树的速度。

在存储占用方面，除去存储点云数据以外，现有方法还需存储每个节点的左右子节点的地址信息，此部分需要的存储大小为2T*log₂T。对于本发明来说，除了点云本身存储，需要的存储大小约为2V(n_x+n_y+n_z)。基于V<<T，因此，本发明的方法减轻了硬件的内存负担。

在查询近邻方面，由于本发明构建的K-D树复杂度降低，查找近邻时回溯K-D树的过程缩短，同时，本发明中用点云数据直接存储的策略取代原技术的间接存储，避免了查询近邻时逐个计算数据点的串行操作，使得算法可利用硬件优势并行处理多个数据，从而实现加速查找。

仿真验证

仿真采用的点云为经过粗配准后的两片斯坦福龙模型。仿真实验分为三组，每组实验采用的点云数目不同。图5为三组实验采用的点云。

对现有技术的仿真，首先用C++代码编写程序并验证，然后在Visual Studio 2017平台(Intel Core i7-2600 CPU@3.4GHz)测试。

对本发明方法的仿真，首先用Verilog HDL语言描述电路，然后仿真验证电路功能，最后在Modelsim平台测试。本实验的时钟频率为100Mhz。

表1为测量K-D树构建和模型点云查找一次最近邻时间的实验结果。

表1

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (6)

1.基于点云配准ICP算法构建K-D树的方法，其特征在于，包括以下步骤：

步骤1、根据点云数据的存储位宽，将点云空间划分为多个子空间并定义子空间坐标；

步骤2、为每个子空间分配地址连续的存储空间，以及1bit的有效标志位；

步骤3、读取点云数据，确定每个数据所属的子空间坐标，将该数据存储在该子空间对应的存储空间，同时将该子空间的有效标志位置为1，该子空间为有效子空间；

步骤4、根据有效子空间建立K-D树，K-D树上的叶子节点均为有效子空间的坐标，非叶子节点均为分割面方向划分有效子空间的分割值；

步骤5、存储K-D树信息，每个非叶子节点存储自身节点的分割值，并对分割值增加两个比特标志位，分别表示该非叶子节点的左右子节点是否为叶节点；每个叶子节点存储自身节点值，即子空间的坐标值。

2.根据权利要求1所述基于点云配准ICP算法构建K-D树的方法，其特征在于，步骤1中划分多个子空间的方法如下：

获取点云数据在x、y、z三个维度上的存储位宽N_x、N_y、N_z，并根据存储位宽确定每个维度的编码位数n_x、n_y、n_z，根据编码位数n_x、n_y、n_z的二进制码对点云空间的x、y、z维度编码，使x、y、z三个维度分别分成

组，将整个点云空间划分为

个子空间，每个子空间的坐标为{S_x,S_y,S_z}；

其中，满足条件，n_x<N_x，n_y<N_y，n_z<N_z；

S_x为子空间在x维度的坐标，S_y为子空间在y维度的坐标，S_z为子空间在z维度的坐标。

3.根据权利要求2所述基于点云配准ICP算法构建K-D树的方法，其特征在于，将存储位宽N_x、N_y和N_z按相同比例缩小后取整得到每个维度的编码位数n_x、n_y和n_z。

4.根据权利要求3所述基于点云配准ICP算法构建K-D树的方法，其特征在于，步骤2中分配地址连续的存储空间的方法如下：

将目标点云的存储空间的地址分为

个区间，使得一个子空间对应一个地址连续的存储区间，记录每个子空间所占用存储空间的起始地址，子空间占用存储的地址范围如下：

S*2^k～(S+1)*2^k-1

其中，S为子空间的编码，k＝N_x+N_y+N_z-n_x-n_y-n_z。

5.根据权利要求1所述基于点云配准ICP算法构建K-D树的方法，其特征在于，步骤4中K-D树的建立方法包括以下步骤：

S1、定义所有有效子空间的坐标为一个数据域，从第一层开始，将数据域的子空间坐标在分割方向排序，找到中位值，该中位值即为根节点，小于中位值的子空间归为根节点的左数据域，大于等于中位值的子空间归为右数据域；

S2、左数据域和右数据域分别在下一层的分割方向排序取中位值，产生当前节点的左子节点和右子节点，重复该步骤，逐层产生分割节点；

S3、当数据域的子空间数目为1时，将该子空间的坐标作为叶子节点输出，当所有子空间的坐标输出后，K-D树建立完成。

6.根据权利要求5所述基于点云配准ICP算法构建K-D树的方法，其特征在于，步骤5中存储K-D树信息的方法如下：

令根节点的存储地址为1，任意一节点的左子节点的存储地址为其父节点的地址左移一位，右子节点的存储地址为其父节点的地址左移一位后加1，得到所有节点的存储地址；

每个非叶子节点用n_x+n_y+n_z+2bit存储，其中最高位和次高位比特分别表示该非叶子节点的左右子节点是否为叶子节点，其余比特表示该非叶子节点的分割值；

每个叶子节点用n_x+n_y+n_z比特来存储，节点值表示有效子空间的坐标。

Images