【三维点云数据处理】⽤python实现KDTree（不使⽤Open3D）python实现KDTree
KDTree简介
KD 树⼜称 K 维树 （K-dimensional tree），是⼀种可以对 K 维数据进⾏划分的数据结构，可以看成⼆元搜索树的⼀种延伸，不断的对空间中的维度做划分，利⽤搜寻树剪枝的特性缩短时间复杂度，主要应⽤在多维空间搜寻，例如最近邻居搜索。
构建KDTree
创建 KD 树的⽅法使⽤：
标准 KD 树分割规则
选择最⼤变异数作为维度选择⽅法
将数据储存在所有节点
KD 树的建⽴过程如下：
1. 在K维数据集合中选择具有最⼤变异数的维度K，然后在该维度上选择中位数m做为分割点对该数据集
合进⾏划分，得到两个⼦集合，
同时建⽴⼀个树节点⽤于储存。
2. 对两个⼦集合重复 （1） 步骤的过程，直到所有⼦集合都不能在划分为⽌。
⽰例
假设有⼀个⼆维阵列：[[2， 3]， [5， 4]， [9， 6]， [4， 7]， [8， 1]， [7， 2]]
1. 分别计算 x， y ⽅向上数据的变异数，得出 x ⽅向上的⽅差最⼤
2. 根据x轴⽅向的值（2，4，5，7，8，9），排序选出中位数为5和7，这⾥选择7当作分割点，所以该节点中的数据为（7，2）。
这样该节点的分割超平⾯就是通过（7， 2）并垂直于x轴的直线x = 7。
3. 确定左⼦空间和右⼦空间。 以x = 7 为分割超平⾯将整个空间分为两个部分，x <= 7 的部分为左⼦空间，包含3个节点（2， 3），
（4， 7）， （5， 4），另⼀部分为右⼦空间，包含2个节点 （8， 1）， （9， 6）。
KD 树的建⽴是⼀个递归的过程。 继续对左⼦空间和右⼦空间内的数据重复根节点的操作就可以得到下⼀级⼦节点 （5， 4） 和（9， 6） （也就是左右⼦空间的根节点），同时将空间和数据进⼀步细分。 如此反复值到空间中只包含⼀个数据点。
Python实现代码
#kdtree的具体实现，包括构建和查找
import copy
#数据结构：保存点的索引和距离
class DistIndex:
def__init__(lf,distance,index):
lf.distance=distance
lf.index=index
仙人掌果实def__lt__(lf,other):
return lf.distance<other.distance
#KNN搜索结果
#输⼊:
#capacity：需要搜索的点的个数
class KNNResultSet:
def__init__(lf,capacity):
lf.capacity=capacity
lf.worst_dist=1e10
lf.dist_index_list=[]
for i in range(capacity):
lf.dist_index_list.append(DistIndex(lf.worst_dist,0))
def size(lf):
unt
def full(lf):
unt==lf.capacity
def worstDist(lf):
用电安全知识return lf.worst_dist
#插⼊点
def add_point(lf,dist,index):
if dist>lf.worst_dist:
return
unt<lf.capacity:
unt-1
while i>0:
if lf.dist_index_list[i-1].distance>dist:
lf.dist_index_list[i]=copy.deepcopy(lf.dist_index_list[i-1])
关于田园的诗句
i-=1
百米赛跑打一成语el:
break
lf.dist_index_list[i].distance=dist
lf.dist_index_list[i].index=index
lf.worst_dist=lf.dist_index_list[lf.capacity-1].distance
def__str__(lf):
output=''
for i,dist_index in enumerate(lf.dist_index_list):
output+='%d - %.2f\n'%(dist_index.index,dist_index.distance)
output+='In total %d comparison operations.'%lf.comparison_counter return output
#RadiusNN搜索结果
#输⼊:
#radius：需要搜索的半径
class RadiusNNResultSet:
class RadiusNNResultSet:
def__init__(lf,radius):
lf.radius=radius
lf.worst_dist=radius
lf.dist_index_list=[]
def size(lf):
unt
def worstDist(lf):
return lf.radius
def add_point(lf,dist,index):
if dist>lf.radius:
return
幼儿园中班评语
找的成语
lf.dist_index_list.append(DistIndex(dist,index))
def__str__(lf):
lf.dist_index_list.sort()
output=''
for i,dist_index in enumerate(lf.dist_index_list):
output+='%d - %.2f\n'%(dist_index.index,dist_index.distance)
output+='In total %d neighbors within %f.\nThere are %d comparison operations.'\ %(lf.count,lf.parison_counter)
return output
import random
import math
import numpy as np
#Node类，Node是tree的基本组成元素
class Node:
def__init__(lf,axis,value,left,right,point_indices):
lf.axis=axis
lf.value=value
lf.left=left
lf.right=right
lf.point_indices=point_indices
def is_leaf(lf):
if lf.value is None:
return True
el:
return Fal
def__str__(lf):
output=''
output+='axis %d, '%lf.axis
if lf.value is None:
output+='split value: leaf, '
el:
output+='split value: %.2f, '%lf.value
动漫女生头像高冷霸气output+='point_indices: '
output+=str(lf.list())
return output
#功能：构建树之前需要对value进⾏排序，同时对⼀个的key的顺序也要跟着改变
#输⼊：
#key：键
#value：值
#输出：
#key_sorted：排序后的键
#value_sorted：排序后的值
def sort_key_by_vale(key,value):
asrt key.shape==value.shape
asrt len(key.shape)==1
sorted_idx=np.argsort(value)
key_sorted=key[sorted_idx]
value_sorted=value[sorted_idx]
return key_sorted,value_sorted
#功能：随机分割轴
#输⼊：
#axis：当前的分割轴
#dim：总维度
def axis_round_robin(axis,dim):
if axis==dim-1:
return0
el:
return axis+1
#功能：通过递归的⽅式构建树
#输⼊：
#root：树的根节点
#db：点云数据
#point_indices：排序后的键
#axis:scalar：初始分割轴
#leaf_size:scalar：叶⼦节点包含的最多数据点
#输出：
#root：即构建完成的树
def kdtree_recursive_build(root,db,point_indices,axis,leaf_size):
if root is None:
root=Node(axis,None,None,None,point_indices)#构建节点
神妙#决定是否分割当前节点
if len(point_indices)>leaf_size:
#获取分割位置
point_indices_sorted,_=sort_key_by_vale(point_indices,db[point_indices, axis])#按给定的轴对于点进⾏排序
#位于分割轴左边的最远⼀个点的索引
middle_left_il(point_indices_sorted.shape[0]/2)-1
middle_left_point_idx=point_indices_sorted[middle_left_index]
middle_left_point_value=db[middle_left_point_idx,axis]
#位于分割轴右边的最近⼀个点的索引
middle_right_index=middle_left_index+1
middle_right_point_idx=point_indices_sorted[middle_right_index]
middle_right_point_value=db[middle_right_point_idx,axis]
#当前节点的值=距离分割值最近的两个点的平均
root.value=(middle_left_point_value+middle_right_point_value)*0.5
#递归构建左边和右边叶⼦节点
root.left=kdtree_recursive_build(root.left,db,point_indices_sorted[0:middle_right_index],axis_chod_by_cov(db[point_indices_sorted[0:middle_right _index]]),leaf_size)
root.right=kdtree_recursive_build(root.right,db,point_indices_sorted[middle_right_index:],axis_chod_by_cov(db[point_indices_sorted[middle_right_ index:]]),leaf_size)
return root
#功能：按照协⽅差的⼤⼩选取分割轴
#输⼊：
#data：需要分割的数据点
def axis_chod_by_cov(data):
axis=0
max_cov=0
for i in range(data.shape[1]):
v(data[:,i])>max_cov:
max_v(data[:,i])
axis=i
return axis
#功能：遍历⼀个kd树
#输⼊：
#root：kd树
#depth：当前深度
#max_depth：最⼤深度
def traver_kdtree(root:Node,depth,max_depth):
depth[0]+=1
if max_depth[0]<depth[0]:
max_depth[0]=depth[0]
if root.is_leaf():
print(root)
el:
traver_kdtree(root.left,depth,max_depth)
traver_kdtree(root.right,depth,max_depth)
depth[0]-=1
#功能：构建kd树（利⽤kdtree_recursive_build功能函数实现的对外接⼝）#输⼊：
#db_np：原始数据N*3维
#leaf_size：scale：叶⼦节点尺⼨
#输出：
#root：构建完成的kd树
def kdtree_construction(db_np,leaf_size):
N,dim=db_np.shape[0],db_np.shape[1]
#build kd_tree recursively
root=None
root=kdtree_recursive_build(root,
db_np,
np.arange(N),
axis=0,
leaf_size=leaf_size)
return root
