Python点云处理——建立KDtree

一、算法原理

        KDtree是计算机科学中一种常用的数据结构算法，它构建出了一种类似于二叉树的树形数据存储结构，每一层都对应原始数据中相应的维度，以K层为一个循环，因此被称为KDtree。而每一层的左右子树的划分依据则是一个人为指定的超平面，该平面对应的坐标为根节点，小于的放在左子树，大于的放在右子树，一直不停的分割下去，将所有维度遍历一遍之后，再从第一个维度重新选择一个超平面开始分割，直至将左右子树分割到都只剩一个叶子节点为止。这样就将所有的数据都在KDtree上唯一确定了位置，以便进行最近邻搜索、半径搜索等操作。

二、计算过程

        下面以一组三维点云数据(1,1,1),(2,2,2),(1,3,1),(3,4,2),(4,5,6)为例：

        首先，x坐标划分，以2为分界，(2,2,2)为根节点，小于2的在左子树：(1,1,1),(1,3,1)，大于2的在右子树：(3,4,2),(4,5,6)；

        然后以y坐标划分，前一层的左子树以1为分界，(1,1,1)在根节点，(1,3,1)在右子树；前一层右子树以4为分界，(3,4,2)在根节点，(4,5,6)在右子树，这样就将所有点云数据唯一地存储在了KDtree中。

三、代码实现

        目前有多种Python实现KDtree的库和方法，比如Scipy和专用于点云处理的PCL、Open3D。为了保证数据的通用性和效率，使用Scipy进行实现。

import numpy as np
from scipy.spatial import KDTree

point= np.random.rand(1000,7)
tree = KDTree(point[:,0:3])

        此处，先随机生成一个7维的点云数据（xyz坐标，xyz法向量，标签），然后调用Scipy中的scipy.spatial.KDTree库函数。与Open3D相比，该库函数可以生成任意维度的KDtree，而不是只能输入三维点云，在处理带有法向量和标签等其他维度的点云数据时具有天然的优势。

四、具体应用

        生成KDtree后，最常见的应用就是对其进行各种搜索。比如在上面的代码中添加一句：

for i in range(0,len(point)):
    neighbors = tree.query_ball_point(point[i,0:3], 0.1,workers=-1,return_length=True)

        这句代码调用了Scipy的KDtree模块中的“球查询”，即半径查找功能，可以查找点云中指定点在半径0.1内的所有近邻点，workers=-1代表启用多线程，poinr[i,0:3]代表只对前三列数据进行查找，若不指定return_lengeth参数，默认返回这些近邻点的索引。若指定其为True，则返回这些点的个数，从而便于进行滤波算法的构建。