稀疏八叉树算法示例：
from matplotlib import pyplot as plt
import numpy as npclass 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) / 2mid_y = (y_min + y_max) / 2mid_z = (z_min + z_max) / 2sub_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:returnfor point in self.points:for child in self.children:if child.contains_point(point):child.points.append(point)breakself.points = [] # 清空当前节点的点，所有点已分配到子节点# 检查点是否在节点的边界内def contains_point(self, point):x, y, z = pointx_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 = 6class 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.pointsself._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()
效果如下：
