稀疏八叉树算法示例:
from matplotlib import pyplot as plt import numpy as np class OctreeNode: def __init__(self, bounds, depth=0): self.bounds = bounds # 体素的空间边界 self.children = [] # 存储子节点 self.depth = depth # 当前节点的深度 self.is_leaf = True # 标记是否为叶节点 self.points = [] # 存储在该节点内的点云数据 # 判断是否需要细分节点 def needs_subdivision(self, max_points): # 当节点中的点数超过最大限制且深度未超过最大深度时细分 return len(self.points) > max_points and self.depth < OctreeNode.MAX_DEPTH # 细分节点,将其分成8个子节点 def subdivide(self): x_min, y_min, z_min = self.bounds['min'] x_max, y_max, z_max = self.bounds['max'] mid_x = (x_min + x_max) / 2 mid_y = (y_min + y_max) / 2 mid_z = (z_min + z_max) / 2 sub_bounds = [ {'min': [x_min, y_min, z_min], 'max': [mid_x, mid_y, mid_z]}, {'min': [mid_x, y_min, z_min], 'max': [x_max, mid_y, mid_z]}, {'min': [x_min, mid_y, z_min], 'max': [mid_x, y_max, mid_z]}, {'min': [mid_x, mid_y, z_min], 'max': [x_max, y_max, mid_z]}, {'min': [x_min, y_min, mid_z], 'max': [mid_x, mid_y, z_max]}, {'min': [mid_x, y_min, mid_z], 'max': [x_max, mid_y, z_max]}, {'min': [x_min, mid_y, mid_z], 'max': [mid_x, y_max, z_max]}, {'min': [mid_x, mid_y, mid_z], 'max': [x_max, y_max, z_max]}, ] for i in range(8): self.children.append(OctreeNode(sub_bounds[i], self.depth + 1)) self.is_leaf = False # 不再是叶节点 # 分配点云到子节点 def distribute_points(self): if self.is_leaf: return for point in self.points: for child in self.children: if child.contains_point(point): child.points.append(point) break self.points = [] # 清空当前节点的点,所有点已分配到子节点 # 检查点是否在节点的边界内 def contains_point(self, point): x, y, z = point x_min, y_min, z_min = self.bounds['min'] x_max, y_max, z_max = self.bounds['max'] return x_min <= x <= x_max and y_min <= y <= y_max and z_min <= z <= z_max # 定义 Octree 的最大深度 OctreeNode.MAX_DEPTH = 6 class SVO: def __init__(self, points, bounds): self.root = OctreeNode(bounds) # 根节点 self.points = points # 输入的点云数据 def build_tree(self, max_points_per_node): self.root.points = self.points self._subdivide(self.root, max_points_per_node) def _subdivide(self, node, max_points_per_node): if node.needs_subdivision(max_points_per_node): node.subdivide() node.distribute_points() for child in node.children: self._subdivide(child, max_points_per_node) def traverse(self, callback): self._traverse_node(self.root, callback) def _traverse_node(self, node, callback): if node.is_leaf: callback(node) else: for child in node.children: self._traverse_node(child, callback) # 绘制框图 def draw_box(ax, min_corner, max_corner): # 绘制一个框图,min_corner和max_corner分别是框图的最小和最大点 min_corner = np.array(min_corner) max_corner = np.array(max_corner) # 定义框图的8个顶点 vertices = [ min_corner, # 0 (min_corner[0], max_corner[1], min_corner[2]), # 1 (min_corner[0], min_corner[1], max_corner[2]), # 2 (min_corner[0], max_corner[1], max_corner[2]), # 3 (max_corner[0], min_corner[1], min_corner[2]), # 4 (max_corner[0], max_corner[1], min_corner[2]), # 5 (max_corner[0], min_corner[1], max_corner[2]), # 6 (max_corner[0], max_corner[1], max_corner[2]), # 7 ] # 定义框图的12条边 edges = [ (0, 1), (1, 5), (5, 4), (4, 0), # 底面 (2, 3), (3, 7), (7, 6), (6, 2), # 顶面 (0, 2), (1, 3), (4, 6), (5, 7), # 侧面 ] # 绘制框图的边 for edge in edges: start_point = vertices[edge[0]] end_point = vertices[edge[1]] ax.plot([start_point[0], end_point[0]], [start_point[1], end_point[1]], [start_point[2], end_point[2]], 'k-') def callback(node): print(node.bounds) # 绘制边界框图 draw_box(ax, node.bounds['min'], node.bounds['max']) print(node.points) # 绘制点 fig = plt.figure() ax = fig.add_subplot(111, projection='3d') if __name__ == '__main__': points = [[1, 2, 3], [4, 5, 6], [7, 8, 9], [10, 11, 12],[13, 14, 15]] bounds = {'min': [-50, -50, -50], 'max': [50, 50, 50]} svo = SVO(points, bounds) svo.build_tree(2) svo.traverse(callback) ax.scatter(*zip(*points)) # 绘制整个边界框图 draw_box(ax, bounds['min'], bounds['max']) # 设置坐标轴的显示范围 ax.set_xlim(-60, 60) ax.set_ylim(-60, 60) ax.set_zlim(-60, 60) # 显示图形 plt.show()
效果如下:
标签:SVO,min,max,八叉树,示例,mid,节点,bounds,self From: https://www.cnblogs.com/hxqmw/p/18496299