前言：本系列文章是关于三维点云处理的常用算法，深入剖析pcl库中相关算法的实现原理，并以非调库的方式实现相应的demo。
1. 最近邻问题概述
（1）最邻近问题：对于点云中的点，怎么去找离它比较近的点
（2）获取邻域点的两种方法：KNN和RNN
- KNN：如图所示，红色点是要查找的点，蓝色点是数据库中的点，图中是找离红色点最近的3个点，显示出来就是图中的绿色点。
- Radius-NN 以上述红色点为圆心，以所选值为半径画圆，圆内的点就是所要找的点
（3）点云最近邻查找的难点 - 点云不规则
- 点云是三维的，比图像高一维，由此造成的数据量是指数上升的。当然，可以建一个三维网格，把点云转化为一个类似于三维图像的东西，但是这也会带来一些矛盾。因为如果网格很多，分辨率足够大，但处理网格需要的内存就很大；如果网格很少，内存够了，但是分辨率又太低。并且，网格中大部分区域都是空白的，所以网格从原理上就不是很高效。
- 点云数据量通常非常大。比如，一个64线的激光雷达，它每秒可产生2.2million个点，如果以20Hz的频率去处理，就意味着每50ms要处理110000个点，如果使用暴力搜索方法对这110000个点都找它最邻近的点，那么计算量为： 110000 × 110000 × 0.5 = 6 × 1 0 9 110000\times110000\times0.5=6\times10^9 110000×110000×0.5=6×109 （4）最近邻查找：BST、Kd-tree、Octree的共同核心思想
- 空间分割 将空间分割成多个部分，然后在每个小区域中去搜索
- 搜索停止条件 若已知目标点到某一点的距离，那么对于超过这一距离的范围就不需要进行搜索，这个距离也被称为"worst distance"
2. 二叉树（Binary Search Tree, BST）
（1）二叉搜索树的特点（一维数据）
- 结点的左子树上的值都小于该根结点的值
- 结点的右子树上的值都大于该根节点的值
- 每一个左右子树都是一个BST
（2）二叉树的构建——给定一串数字，如何构造出一个BST
classNode:def__init__(self, key, value=-1):
self.left =None
self.right =None
self.key = key
self.value = value # 这里的value表示当前点的其他属性，比如颜色、编号等
Data generation —— 随机产生一串数字
db_size =10
data = np.random.permutation(db_size).tolist()
Recursively insert each an element —— 构造BST的具体实现
definsert(root, key, value=-1):if root isNone:
root = Node(key, value)else:if key < root.key:
root.left = insert(root.left, key, value)elif key > root.key:
root.right = insert(root.right, key, value)else:passreturn root
Insert each element —— 主函数调用
root =Nonefor i point inenumerate(data):
root = insert(root, point, i)# 这里的value（i）表示的是当前点在原始数组中的位置
（3）BST的复杂度
- 最坏情况下，二叉树的各结点顺次链接，排成一列，此时复杂度为 O ( h ) O(h) O(h)，其中 h h h为BST的高度，也是BST中结点个数
- 最好情况下，BST是处于平衡状态的，此时复杂度为 O ( l o g 2 n ) O(log_2n) O(log2n)， n n n为BST中结点总数
（4）二叉树查找——对于一个给定的点（数值），查找它是否在BST中
# 递归法defsearch_recursive(root, key):if root isNoneor root.key == key:return root
if key < root.key:# 表明key在当前的左子树上return search_recursive(root.left, key)elif key > root.key:# 表明key在当前的右子树上return search_recursive(root.right, key)# 迭代法 —— 通过栈来实现（不断迭代更新current_node）defsearch_iterative(root, key):
current_node = root
while current_node isnotNone:if current_node.key == key:return current_node
elif current_node.key < key:
current_node = current_node.right
elif current_node.key > key:
current_node = current_node.left
return current_node
（5）递归法与迭代法的特点
- 递归
好处：实现简单，容易理解，代码简短
坏处：由于递归需要不停地去压栈，所以每一次递归就是在内存中记录当前递归的位置，因此递归需要
O
(
n
)
O(n)
O(n)的内存空间，这里的
n
n
n就是递归的次数
- 迭代
优点：它用一个量current_node来表示当前的位置，因此它所需的存储空间为
O
(
1
)
O(1)
O(1)；另外，由于GPU对于堆栈是比较困难的，往往只支持20多层的堆栈，很多时候是不够用的，可能会造成栈溢出（stack-overflow），而且在GPU上实现递归是非常慢的，迭代法可以避免这一问题
缺点：实现起来较为困难
（6）深度优先搜索 （Depth First Traversal）
# 前序遍历 —— 可用于复制一棵树defpreorder(root):if root isnotNone:print(root)
preorder(root.left)
preorder(root.right)# 中序遍历 —— 可用于排序definorder(root):if root isnotNone:
inorder(root.left)print(root)
inorder(root.right)# 后序遍历 —— 可用于删除一个结点defpostorder(root):if root isnotNone:
postorder(root.left)
postorder(root.right)print(root)
（7）KNN——寻找K个最近邻的点
寻找当前点的K个最近邻点关键在于如何确定worst dist，具体步骤如下：
- 建立一个容器container来存储当前KNN的结果，并将容器中的结果进行排序：例如，当K = 6时，当前KNN结果为[1, 2, 3, 4, 4.5, inf]
- 容器中最后一个就是worst dist，对于新增结点，若新增结点与当前结点计算出来的dist小于当前worst dist，则将其添加到容器中，同时更新worst：例如，若新增结点计算出来的dist为6，则容器中先腾出空间[1, 2, 3, 4, 4.5, 4.5]，然后再将当前dist放入到容器中，结果为[1, 2, 3, 4, 4.5, 6]，此时worst dist为6
代码实现：构建容器KNNReslutSet
classDistIndex:def__init__(self, distance, index):
self.distance = distance
self.index = index
def__lt__(self, other):return self.distance < other.distance
classKNNResultSet:'''
用于存储KNN查找结果的容器
capacity: 容器大小
count: 当前容器中的元素个数
worst_dist: 容器结果中的最大值(最长距离)
dist_index_list: 容器中各个结果距离所对应的结点序号
'''def__init__(self, capacity):
self.capacity = capacity
self.count =0
self.worst_dist =1e10# 初始时，将容器中的数据设置大一些
self.dist_index_list =[]for i inrange(capacity):
self.dist_index_list.append(DistIndex(self.worst_dist,0))
self.comparison_counter =0defsize(self):return self.count
deffull(self):return self.count == self.capacity
defworstDist(self):return self.worst_dist
defadd_point(self, dist, index):
self.comparison_counter +=1if dist > self.worst_dist:returnif self.count < self.capacity:
self.count +=1# 若当前容器元素个数 小于 容器容量，则新增空位
i = self.count -1# 因为是从0开始索引的，故将i定位到新增空位处的索引上# 下面其实是将容器中的元素进行排序(包括新腾出来的空位)，排序结果根据dist对应的index进行存储while i >0:# 若当前容器的最大距离(最后索引) 大于 当前新增距离dist (上面的4.5与6进行比较)if self.dist_index_list[i-1].distance > dist:# 则将其往后挪一位，使得整体上是从大到小的顺序
self.dist_index_list[i]= copy.deepcopy(self.dist_index_list[i-1])
i -=1else:break# 否则，跳出循环，将当前dist放到最后一个元素后面
self.dist_index_list[i].distance = dist
self.dist_index_list[i].index = index
self.worst_dist = self.dist_index_list[self.capacity-1].distance
KNN查找:在当前以Node为根节点(root)的二叉树中，查找离key最近的K个结点，并将其存储于result_set中
defknn_search(root:Node, result_set:KNNResultSet, key):if root isNone:returnFalse# 将根节点root与目标节点key进行比较，也就是将当前根节点root放到容器中，这里root.key - key表示dist，root.value表示index
result_set.add_point(math.fabs(root.key - key), root.value)if result_set.worstDist()==0:# 若worstDist为0，表示当前根节点root就是所要找到的节点keyreturnTrue# 若当前根节点 大于 目标节点if root.key >= key:# 则在当前根节点的左子树上进行同样的查找操作if knn_search(root.left, result_set, key):returnTrue# 若当前根节点与目标节点的差 小于 最坏距离，那么还可以寻找较大的节点，使其与目标节点的差在最坏距离之内，因此可往当前根节点的右子树上寻找节点elif math.fabs(root.key - key)< result_set.worstDist():return knn_search(root.right, result_set, key)returnFalseelse:# 反之，若当前根节点 小于 目标节点，则在当前根节点的右子树上进行同样的查找操作if knn_search(root.right, result_set, key):returnTrue# 若当前根节点与目标节点的差 小于 最坏距离，那么还可以寻找较小的节点，使其与目标节点的差在最坏距离之内，因此可往当前根节点的左子树上寻找节点elif math.fabs(root.key - key)< result_set.worstDist():return knn_search(root.left, result_set, key)returnFalse
（8）Radius NN查找
Radius NN查找中worstDist是固定的，因此不需要容器存储查找结果，并通过排序维持worstDist，只需将当前节点和目标节点的差与worstDist进行判断，来确定当前节点是否为目标节点的Radius最近点。
# 新增节点defadd_point(self, dist, index):
self.comparison_counter +=1if dist > self.radius:# 若当前节点计算出的dist大于radius，则退出(不将其加入到最近点范围内)return# 反之，若当前节点计算出的dist 小于 radius，则将计算出的dist及该节点的索引index存储到最近点范围# 同时存储该点的索引index
self.count +=1
self.dist_index_list.append(DistIndex(dist, index))# Radius NN查找defradius_search(root:Node, result_set:RadiusNNResultSet, key):if root isNone:returnFalse# 计算当前根节点root.key与目标节点key的差，判断它是否可以放入容器中result_set
result_set.add_point(math.fabs(root.key - key), root.value)# 同KNN一样，若当前根节点 大于 目标节点if root.key >= key:# 则在当前根节点的左子树上进行同样的查找操作if radius_search(root.left, result_set, key):returnTrue# 若当前根节点与目标节点的差 小于 最坏距离，那么还可以寻找较大的节点，使其与目标节点的差在最坏距离之内，因此可往当前根节点的右子树上寻找节点elif math.fabs(root.key - key)< result_set.worstDist():return radius_search(root.right, result_set, key)returnFalseelse:# 反之，若当前根节点 小于 目标节点，则在当前根节点的右子树上进行同样的查找操作if radius_search(root.right, result_set, key):returnTrue# 若当前根节点与目标节点的差 小于 最坏距离，那么还可以寻找较小的节点，使其与目标节点的差在最坏距离之内，因此可往当前根节点的左子树上寻找节点elif math.fabs(root.key - key)< result_set.worstDist():return radius_search(root.left, result_set, key)returnFalse
（9）二叉树的应用
二叉树一般是应用在一维数据上，不能将其应用在高维数据上。因为二叉树的查找依赖于左边小、右边大这一特性，对于一维数据来说，这种大和小可以通过直接比较查询点与当前点的大小来获取。而对于高维数据，在某一维度上的大小并不能代表查询点与当前点的大小，故不能应用于高维数据。
3. Kd-tree
kd-tree通常应用于高维空间（任意维）中，在每个维度上进行一次二叉树，就称为Kd-tree。每次划分时，在每个维度的某一超平面上将样本划分成两部分。另外，通过设置leaf-size来定义停止划分空间的条件，若当前结点个数小于leaf-size，则停止划分。其中，每次划分时的维度可以轮流切换，即x-y-z-x-y-z。
以二维为例，设置leaf-size=1，如下图所示：
（1）如何表达Kd-tree的结点
classNode:def__init__(self, axis, value, left, right, point_indices):
self.axis = axis # 当前要切分的维度，比如说接下来是要垂直于哪个轴进行切分
self.value = value # 定义当前所要切分维度中的位置，对应上面就是y轴上的位置
self.left = left # 定义当前结点的左节点
self.right = right # 定义当前结点的右节点
self.point_indices = point_indices # 存储属于当前划分区域的结点的序号# 若当前结点为一个leaf，则没有必要继续往下划分defis_leaf(self):# 因为由下面可知，在构建kdtree过程中，value是初始化为None的if self.value isNone:returnTrueelse:returnFalse
（2）构建kd-tree
defkdtree_recursive_build(root, db, point_indices, axis, leaf_size):'''
root: 所创建的kd-tree的根节点
db: 源数据集
point_indices: 点的索引
axis: 所划分的维度
leaf_size: 最小节点的大小
'''if root isNone:# 若根节点为空，则根据节点的属性创建根节点
root = Node(axis,None,None,None, point_indices)# 若所要划分的样本数量 大于 最小样本数，则进行kd-tree划分，如下：iflen(point_indices)> leaf_size:# 以某一个维度(axis)将db中的点进行排序(这里的排序算法sort_key_by_value是如何得到的？)
point_indices_sorted, _ = sort_key_by_value(point_indices, db[point_indices, axis])# 通过中间靠左位置获取排序后的中间左点(索引及其值)
middle_left_idx = math.ceil(point_indices_sorted.shape[0]/2)-1
middle_left_point_idx = point_indices_sorted[middle_left_idx]
middle_left_point_value = db[middle_left_point_idx, axis]# 通过中间靠右位置获取排序后的中间右点(索引及其值)
middle_right_idx = middle_left_idx +1
middle_right_point_idx = point_indices_sorted[middle_right_idx]
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_idx],
axis_round_robin(axis, dim=db.shape[1]),
leaf_size)# 通过中间右点循环构建右子树
root.right = kdtree_recursive_build(root.right,
db, point_indices_sorted[middle_right_idx:],
axis_round_robin(axis, dim=db.shape[1]),
leaf_size)return root
# 如何选取分割维度：这里采用的是轮换分割，即x-y-x-ydefaxis_round_robin(axis, dim):if axis == dim-1:return0else:return axis +1
（3）复杂度
假设建立的kd-tree有
n
n
n个点，并且在每个维度上是均匀平衡的，那么总共有
l
o
g
n
logn
logn层，在每一层中进行排序，使得小于的放左边，大于的放右边，这个排序算法的实践复杂度是
O
(
n
l
o
g
n
)
O(nlogn)
O(nlogn)，因此，总的时间复杂度是：
O
(
n
l
o
g
n
l
o
g
n
)
O(nlognlogn)
O(nlognlogn)。对于空间复杂度，首先，由于每一层中对于每个结点都要存储其index，所以需要
O
(
n
l
o
g
n
)
O(nlogn)
O(nlogn)，总的空间复杂度是
O
(
k
n
+
n
l
o
g
n
)
O(kn+nlogn)
O(kn+nlogn)。
（4）使用已经构建好的kdtree进行knn查找
关键在于：对于给定查询点query，判断是否要搜索某一区域。若给定的查询点query在某一区域内，或者给定的查询点query到该区域的最小距离 小于 当前的worst dist，那么就需要在这个区域内进行knn查找
defknn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray):'''
root: kd-tree的根节点
db: 源数据集
result_set: 用于存储knn查找结果的容器，里面维持从小到大的顺序，最后面最大的作为worstDist
query: 待查询点
'''if root isNone:returnFalse# 判断当前结点是否为叶子节点，若是，则使用暴力查找方法在叶子节点区域中进行查找# 因为kd-tree中的叶子节点区域可能不是一个节点，而是多个节点，那么此时就在这多个节点中进行暴力查找if root.is_leaf():
leaf_points = db[root.point_indices,:]
diff = np.linalg.norm(np.expand_dims(query,0)- leaf_points, axis=1)for i inrange(diff.shape[0]):
result_set.add_point(diff[i], root.point_indices[i])returnFalse# 若在当前axis维度上，待查询点值小于根节点值，则待查询点在根节点的当前维度的左侧if query[root.axis]<= root.value:# 则需要在左子树上查找
knn_search(root.left, db, result_set, query)# 另外，右子树上也可能存在离待查询点query较近的点，那么也需要在当前右子树上进行查找。前提是：query在当前维度的值小于根节点的值，且二者之差比最坏距离worstDist小 ### 这一块还不太懂if math.fabs(query[root.axis]- root.value)< result_set.worstDist():
knn_search(root.right, db, result_set, query)else:
knn_search(root.right, db, result_set, query)if math.fabs(query[root.axis]- root.value)< result_set.worstDist():
knn_search(root.left, db, result_set, query)returnFalse
（5）Kd-tree中的Radius NN
同KNN中的Radius NN一样，区别在于此时的worse dist是固定的，而不是在每次分割中需要根据查找点的距离动态更新的。
4. Octree
（1）Octree特点
针对于三维数据，每个维度进行划分，分割一次会得到8个部分。Octree的好处是：高效，不需要经过根节点就可以提前终止划分。因为：若以查询点为中心，以固定值为半径形成一个球，如果这个球完全落在了某个立方体内，那么此时的搜索范围就可以限定在这个小立方体内。而kd-tree中每一层结点只考虑了一个维度，在当前维度下不知道其他维度是如何分割的，所以只有一个维度信息是不足以确定什么时候可以终止搜索。
（2）Octree的关键流程
- 首先，根据所有点的边界确定最大立方体
- 设置停止搜索条件。主要是设置leaf_size（当前区域中点的个数）；设置最小区域（此时最小区域中可能有多个点重叠在一起），设置最小区域的初衷是：在某些情况下，某一区域中可能存在重复点，此时不需要划分到每个区域中只包含一个点，将这些重复点划分到一个区域中即可。另外，如果在这种情况下不设置最小区域，那么在leaf_size下，相同点（重叠点）是不会划分开的，此时在划分时可能会陷入死循环
（3）创建Octree
octree中节点的结构——octant
classOctant:def__init__(self, children, center, extent, point_indices, is_leaf):
self.children = children # 每次划分时的子节点(8个)
self.center = center # 中心点
self.extent = extent # 半个边长(从中心点到其中一个面的距离)
self.point_indices = point_indices # 点的索引
self.is_leaf = is_leaf # 是否为叶子节点区域
创建octree
defoctree_recursive_build(root, db, center, extent, point_indices, leaf_size, min_extent):'''
root: 要构建的octree的根节点
db: 源数据样本点
center: 中心点
point_indices: 各点的索引
leaf_size: 叶子节点区域大小(叶子节点区域中节点个数)
min_extent: 最小区域大小
'''iflen(point_indices)==0:# 若源数据样本中没有点returnNone# 则返回一个空的octreeif root isNone:# 若初始时octree的根节点为空# 则根据octant节点的属性，创建初始root节点
root = Octant([Nonefor i inrange(8)], center, extent, point_indices, is_leaf=True)# 判断是否需要建立Octree —— 若结点总数小于所设置的min_extent（最小元素个数），就不需要建立iflen(point_indices)<= leaf_size or extent <= min_extent:
root.is_leaf =True# 否则，进行octree的划分else:
root.is_leaf =False# 首先，表明当前节点并不是leaf_size
children_point_indices =[[]for i inrange(8)]# 准备8个子空间# 下面是通过for循环将每个点放入到对应的子空间中for point_idx in point_indices:
point_db = db[point_idx]# 根据索引取出当前点
morton_code =0# 根据当前点与中心点在3个维度上的比较，将空间划分为8份if point_db[0]> center[0]:
morton_code = morton_code |1if point_db[1]> center[1]:
morton_code = morton_code |2if point_db[2]> center[2]:
morton_code = morton_code |4# 将当前点归属到对应的子空间中
children_point_indices[morton_code].append(point_idx)# 创建children
factor =[-0.5,0.5]for i inrange(8):# 计算每一个子节点的中心点的3个维度坐标
child_center_x = center[0]+ factor[(i &1)>0]* extent
child_center_y = center[1]+ factor[(i &2)>0]* extent
child_center_z = center[2]+ factor[(i &4)>0]* extent
# 计算每一个子节点的边长
child_extent =0.5* extent
# 确定每一个子节点的中心点坐标
child_center = np.asarray([child_center_x, child_center_y, child_center_z])# 根据octant参数，递归创建子octree
root.children[i]= octree_recursive_build(root.children[i],
db,
child_center,
child_extent,
children_point_indices[i],
leaf_size,
min_extent)return root
（4）Octree的KNN查找
definside(query: np.ndarray, radius:float, octant: Octant):"""
功能：判断以待查询点query为球心，radius为半径的球是否在Octant中
query: 待查询点
radius: 球半径
octant: 待比较的子区域
"""
query_offset = query - octant.center
query_offset_abs = np.fabs(query_offset)# 当前待查询点query到octant中心的绝对距离
possible_space = query_offset_abs + radius # 上述绝对距离加上半径# 比较绝对距离加半径与 octant一半边长的大小 若小于则表示query在octant内，反之则表示query在octant外return np.all(possible_space < octant.extent)defoverlaps(query: np.ndarray, radius:float, octant: Octant):"""
需要仔细琢磨
功能：判断一个立方体与一个球是否有交集
query: 待查询点
radius: 球半径
octant: 待比较的子区域
"""
query_offset = query - octant.center
query_offset_abs = np.fabs(query_offset)# 当前待查询点query到octant中心的绝对距离
max_dist = radius + octant.extent # 将球半径与当前octant一半边长作为最大距离阈值if np.any(query_offset_abs > max_dist):# case1 判断是否相离returnFalseif np.sum((query_offset_abs < octant.extent).astype(np.int))>=2:# case2 球与面是否相交returnTrue# case3：比较对角线+球半径 与 octant中心到球心之间的距离，来判断球与立方体角点是否相交# 另外，通过max来减少一个维度，使其能够判断球是否与立方体的棱边相交
x_diff =max(query_offset_abs[0]- octant.extent,0)
y_diff =max(query_offset_abs[1]- octant.extent,0)
z_diff =max(query_offset_abs[2]- octant.extent,0)return x_diff * x_diff + y_diff * y_diff + z_diff * z_diff < radius * radius
defoctree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray):"""
root: 创建的octree根节点
db: 源数据样本点
result_set: 存储搜索结果的容器
query: 待查询点
"""if root isNone:# 若Octree为空，则直接返回FalsereturnFalse# 若当前区域为is_leaf，则直接将其中的点与待查询点进行逐个比较————暴力查找if root.is_leaf andlen(root.point_indices)>0:
leaf_points = db[root.point_indices,:]# 取出叶子节点区域中的全部点# 计算待查询点与所有叶子节点之间的距离
diff = np.linalg.norm(np.expand_dims(query,0)- leaf_points, axis=1)# 根据计算出的距离diff，来判断是否可以将对应的点放入到存储容器result_set中for i inrange(diff.shape[0]):
result_set.add_point(diff[i], root.point_indices[i])return inside(query, result_set.worstDist(), root)# 这个inside函数表示的是什么意思# 若当前区域不是is_leaf，则要找当前结点下的8个子节点# 下面的3个if是找到最有可能包含待查询结点的子节点区域
morton_code =0if query[0]> root.center[0]:
morton_code = morton_code |1if query[1]> root.center[1]:
morton_code = morton_code |2if query[2]> root.center[2]:
morton_code = morton_code |4# 在找到的子节点区域中继续递归下去找，若能够返回True，则表示在当前子节点区域下找到了if octree_knn_search(root.children[morton_code], db, result_set, query):returnTrue# 若返回的是False，则表示上面那个子节点区域中没有找到,需要继续在其他子节点区域中查找for c, child inenumerate(root.children):# 遍历到上面已经查找过的子节点区域(morton_code) 或者 遍历到的子节点区域为空时# 直接跳过，在下一个子节点区域中进行查找if c == morton_code or child isNone:continue# overlaps是判断当前octant 与 以待查询点query为圆心、worstDist为半径的球是否有交集————相离# 若没有交点(返回的是False)，则跳过当前子区域octant不进行查找ifFalse== overlaps(query, result_set.worstDist(), child):continue# 在剩下的子区域中进行查找if octree_knn_search(child, db, result_set, query):returnTrue# 若以待查询点为圆心、worstDist为半径的球被octant包围了，那么就可提前终止搜索# inside()：表示一个球是否完全被一个立方体所包围return inside(query, result_set.worstDist(), root)
（5）Octree的KNN改进查找
若可判断出以当前查询点query为球心、worseDist为半径的球包含一个子区域，那么就不必在其他子区域中查找，只需在这个被包围的子区域中查找即可
defcontains(query: np.ndarray, radius:float, octant: Octant):"""
功能：判断以query为球心，radius为半径的球是否包围子区域octant
query: 待查询点
radius: 球心
octant: 子区域
"""
query_offset = query - octant.center
query_offset_abs = np.fabs(query_offset)# 当前待查询点query到octant中心的绝对距离# 将绝对距离 + 当前octant一半长度 作为 最大距离阈值，如下图所示
query_offset_to_farthest_corner = query_offset_abs + octant.extent
return np.linalg.norm(query_offset_to_farthest_corner)< radius
本文转载自: https://blog.csdn.net/LDST_CSDN/article/details/130378074
版权归原作者 LDST_CSDN 所有， 如有侵权，请联系我们删除。
版权归原作者 LDST_CSDN 所有， 如有侵权，请联系我们删除。