三维点云学习(2)下-Octree
Octree(八叉树)的基本结构
思想:立体空间的分割
二维平面的展示
Octree(八叉树)的构建
#extent:立方体中心点到面的距离
#Center of the cube 立方体的中心
1.判断每一个点里面都有怎么样的主节点
2.计算子节点的中心在哪,子节点的边长,再把属于这个子节点的数据放进去
Octant构建完成之后,进行kNN的搜寻
这里展现了Octree比kd-tree的优越在于当红色框被S2包围时,可以马上结束搜寻
1.如果octant是一个末尾叶子节点,直接把数据扔进去KNN搜寻
2.如果不是末尾节点,进行寻找最有可能的子节点
3.如果查找成功,直接退出返回True,不成功,再搜寻其他子节点
4.overlaps() 用于判断,子节点与球是否有交际,没有就跳过(空间结构思想)
5.inside() ,如果球面被某一个Octant包围,可直接退出
inside的实现
#红色代表worst——dist
#正方形代表一个octant
只需要判断 extent > radius +diff 就可认为球在正方体里面
overlaps的实现
#case1:判断是否有接触
如果 diff > extent + radius 可认为球面与octant没接触
#case2:判断球体和某一个面是否接触
假设case1 成立,当有球体的中心点有两个轴位于正方体的相应轴内部时,也同样可判断为有交集
#case3.1:判断球体有没有跟正方体某个角点接触
当 diff < radius 可判断为和角点有接触
#case3.2:判断球体有没有跟正方体某个棱是否接触
注意:用max(,0)解决球体中某个点会陷在正方体的某个轴上,而出现的负数影响
oerlaps回顾:
Better one:
如果有一个球体完全包围一个正方体,那只需要对相应的octant进行kNN search就行
图示:
如果:
图示绿色线 < radius 可认为被包裹
复杂度
完整代码
#运行结果
Radius search normal:
Search takes 45438.469ms
Radius search fast:
Search takes 20330.457ms
import random
import math
import numpy as np
import time
from lidar_KnnRnnClass_2 import KNNResultSet, RadiusNNResultSet
class Octant:
def __init__(self, children, center, extent, point_indices, is_leaf):
self.children = children
self.center = center
self.extent = extent
self.point_indices = point_indices
self.is_leaf = is_leaf
def __str__(self):
output = ''
output += 'center: [%.2f, %.2f, %.2f], ' % (self.center[0], self.center[1], self.center[2])
output += 'extent: %.2f, ' % self.extent
output += 'is_leaf: %d, ' % self.is_leaf
output += 'children: ' + str([x is not None for x in self.children]) + ", "
output += 'point_indices: ' + str(self.point_indices)
return output
def traverse_octree(root: Octant, depth, max_depth):
depth[0] += 1
if max_depth[0] < depth[0]:
max_depth[0] = depth[0]
if root is None:
pass
elif root.is_leaf:
print(root)
else:
for child in root.children:
traverse_octree(child, depth, max_depth)
depth[0] -= 1
def octree_recursive_build(root, db, center, extent, point_indices, leaf_size, min_extent):
if len(point_indices) == 0:
return None
if root is None:
root = Octant([None for i in range(8)], center, extent, point_indices, is_leaf=True)
# determine whether to split this octant
if len(point_indices) <= leaf_size or extent <= min_extent:
root.is_leaf = True
else:
root.is_leaf = False
children_point_indices = [[] for i in range(8)]
for point_idx in point_indices:
point_db = db[point_idx]
morton_code = 0
if point_db[0] > center[0]:
morton_code = morton_code | 1
if point_db[1] > center[1]:
morton_code = morton_code | 2
if point_db[2] > center[2]:
morton_code = morton_code | 4
children_point_indices[morton_code].append(point_idx)
# create children
factor = [-0.5, 0.5]
for i in range(8):
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])
root.children[i] = octree_recursive_build(root.children[i],
db,
child_center,
child_extent,
children_point_indices[i],
leaf_size,
min_extent)
return root
def inside(query: np.ndarray, radius: float, octant:Octant):
"""
Determines if the query ball is inside the octant
:param query:
:param radius:
:param octant:
:return:
"""
query_offset = query - octant.center
query_offset_abs = np.fabs(query_offset)
possible_space = query_offset_abs + radius
return np.all(possible_space < octant.extent)
def overlaps(query: np.ndarray, radius: float, octant:Octant):
"""
Determines if the query ball overlaps with the octant
:param query:
:param radius:
:param octant:
:return:
"""
query_offset = query - octant.center
query_offset_abs = np.fabs(query_offset)
# completely outside, since query is outside the relevant area
max_dist = radius + octant.extent
if np.any(query_offset_abs > max_dist):
return False
# if pass the above check, consider the case that the ball is contacting the face of the octant
if np.sum((query_offset_abs < octant.extent).astype(np.int)) >= 2:
return True
# conside the case that the ball is contacting the edge or corner of the octant
# since the case of the ball center (query) inside octant has been considered,
# we only consider the ball center (query) outside octant
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
def contains(query: np.ndarray, radius: float, octant:Octant):
"""
Determine if the query ball contains the octant
:param query:
:param radius:
:param octant:
:return:
"""
query_offset = query - octant.center
query_offset_abs = np.fabs(query_offset)
query_offset_to_farthest_corner = query_offset_abs + octant.extent
return np.linalg.norm(query_offset_to_farthest_corner) < radius
def octree_radius_search_fast(root: Octant, db: np.ndarray, result_set: RadiusNNResultSet, query: np.ndarray):
if root is None:
return False
if contains(query, result_set.worstDist(), root):
# compare the contents of the octant
leaf_points = db[root.point_indices, :]
diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1)
for i in range(diff.shape[0]):
result_set.add_point(diff[i], root.point_indices[i])
# don't need to check any child
return False
if root.is_leaf and len(root.point_indices) > 0:
# compare the contents of a leaf
leaf_points = db[root.point_indices, :]
diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1)
for i in range(diff.shape[0]):
result_set.add_point(diff[i], root.point_indices[i])
# check whether we can stop search now
return inside(query, result_set.worstDist(), root)
# no need to go to most relevant child first, because anyway we will go through all children
for c, child in enumerate(root.children):
if child is None:
continue
if False == overlaps(query, result_set.worstDist(), child):
continue
if octree_radius_search_fast(child, db, result_set, query):
return True
return inside(query, result_set.worstDist(), root)
def octree_radius_search(root: Octant, db: np.ndarray, result_set: RadiusNNResultSet, query: np.ndarray):
if root is None:
return False
if root.is_leaf and len(root.point_indices) > 0:
# compare the contents of a leaf
leaf_points = db[root.point_indices, :]
diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1)
for i in range(diff.shape[0]):
result_set.add_point(diff[i], root.point_indices[i])
# check whether we can stop search now
return inside(query, result_set.worstDist(), root)
# go to the relevant child first
morton_code = 0
if query[0] > root.center[0]:
morton_code = morton_code | 1
if query[1] > root.center[1]:
morton_code = morton_code | 2
if query[2] > root.center[2]:
morton_code = morton_code | 4
if octree_radius_search(root.children[morton_code], db, result_set, query):
return True
# check other children
for c, child in enumerate(root.children):
if c == morton_code or child is None:
continue
if False == overlaps(query, result_set.worstDist(), child):
continue
if octree_radius_search(child, db, result_set, query):
return True
# final check of if we can stop search
return inside(query, result_set.worstDist(), root)
def octree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray):
if root is None:
return False
if root.is_leaf and len(root.point_indices) > 0: #如果是末尾节点(leaf)那就直接把数据扔进KNNResult
# compare the contents of a leaf
leaf_points = db[root.point_indices, :]
diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1)
for i in range(diff.shape[0]):
result_set.add_point(diff[i], root.point_indices[i])
# check whether we can stop search now
return inside(query, result_set.worstDist(), root)
# go to the relevant child first
morton_code = 0
if query[0] > root.center[0]:
morton_code = morton_code | 1
if query[1] > root.center[1]:
morton_code = morton_code | 2
if query[2] > root.center[2]:
morton_code = morton_code | 4
if octree_knn_search(root.children[morton_code], db, result_set, query):
return True
# check other children
for c, child in enumerate(root.children):
if c == morton_code or child is None:
continue
if False == overlaps(query, result_set.worstDist(), child):
continue
if octree_knn_search(child, db, result_set, query):
return True
# final check of if we can stop search
return inside(query, result_set.worstDist(), root)
def octree_construction(db_np, leaf_size, min_extent):
N, dim = db_np.shape[0], db_np.shape[1]
db_np_min = np.amin(db_np, axis=0)
db_np_max = np.amax(db_np, axis=0)
db_extent = np.max(db_np_max - db_np_min) * 0.5
db_center = db_np_min + db_extent
root = None
root = octree_recursive_build(root, db_np, db_center, db_extent, list(range(N)),
leaf_size, min_extent)
return root
def main():
# configuration
db_size = 64000
dim = 3
leaf_size = 4
min_extent = 0.0001
k = 8
db_np = np.random.rand(db_size, dim)
root = octree_construction(db_np, leaf_size, min_extent)
# depth = [0]
# max_depth = [0]
# traverse_octree(root, depth, max_depth)
# print("tree max depth: %d" % max_depth[0])
# query = np.asarray([0, 0, 0])
# result_set = KNNResultSet(capacity=k)
# octree_knn_search(root, db_np, result_set, query)
# print(result_set)
#
# diff = np.linalg.norm(np.expand_dims(query, 0) - db_np, axis=1)
# nn_idx = np.argsort(diff)
# nn_dist = diff[nn_idx]
# print(nn_idx[0:k])
# print(nn_dist[0:k])
begin_t = time.time()
print("Radius search normal:")
for i in range(100):
query = np.random.rand(3)
result_set = RadiusNNResultSet(radius=0.5)
octree_radius_search(root, db_np, result_set, query)
# print(result_set)
print("Search takes %.3fms\n" % ((time.time() - begin_t) * 1000))
begin_t = time.time()
print("Radius search fast:")
for i in range(100):
query = np.random.rand(3)
result_set = RadiusNNResultSet(radius = 0.5)
octree_radius_search_fast(root, db_np, result_set, query)
# print(result_set)
print("Search takes %.3fms\n" % ((time.time() - begin_t)*1000))
if __name__ == '__main__':
main()