1. 算法原理

八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，将八个子节点所表示的体积元素加在一起就等于父节点的体积。八叉树是四叉树在三维空间上的扩展，二维上我们有四个象限，而三维上，我们有8个卦限。八叉树主要用于空间划分和最近邻搜索。

Building an orthtree in 3D (octree) from a point cloud

实现Octree的原理

将当前的立方体细分为八个子立方体。
如果任何一个子立方体内包含多个点，则将其进一步细分为八个子立方体。
重复以上操作使得每个子立方体内包含最多一个点。

2. 实现

class OccupancyVoxelTree:
# only root node's parent node is None
def __init__(self, min_voxel_sizes=np.ones(3), parent_node=None, depth=0):self.min_voxel_sizes = min_voxel_sizesself.num_points = 0self.points = np.array([])self.center = np.zeros(3, float)self.min_values = np.zeros(3, float)self.max_values = np.zeros(3, float)self.parent_node = parent_nodeself.depth = depthself.voxel_indices = np.zeros(3, int)self.child_nodes = []self.is_leaf = Falsedef get_tree_points(self):if self.is_leaf:return [self.points]else:points = []for node in self.child_nodes:if node is not None:points.extend(node.get_tree_points())return points# only root node call this method
def build_tree(self, points):min_values = np.min(points, axis=0)max_values = np.max(points, axis=0)center = (min_values + max_values) * 0.5num_grids = np.max((max_values - min_values) / self.min_voxel_sizes)num_grids = 2 ** int(math.ceil(math.log2(num_grids)))half_sizes = self.min_voxel_sizes * (num_grids * 0.5)self.min_values = center - half_sizesself.max_values = center + half_sizesself.build_child_tree(points)def empty(self):return self.num_points == 0def get_all_leaves(self):if self.is_leaf:return [self]else:leaves = []for node in self.child_nodes:if node is not None:leaves.extend(node.get_all_leaves())return leavesdef get_neighbors(self):indices_from_root = self.get_indices_from_root()neighbors = []for i in range(-1, 2):for j in range(-1, 2):for k in range(-1, 2):voxel_indices = self.voxel_indices + np.array([i, j, k])neighbors.append(self.parent_node.get_voxel(voxel_indices, 1))return neighborsdef get_voxel(self, indices, depth_from_this_node):if depth_from_this_node > 1:half_octave = 2 ** (depth_from_this_node - 1)octave = half_octave * 2if np.any(indices < 0) or np.any(indices >= octave):voxel_indices = self.voxel_indices * 2 + indicesreturn self.parent_node.get_voxel(voxel_indices, depth_from_this_node + 1)else:child_indices = indices // half_octaveindices_from_child = indices - child_indices * half_octavechild_node = self.get_child_node(child_indices)if child_node is None:return Noneelse:return child_node.get_voxel(indices_from_child, depth_from_this_node - 1)elif self.is_leaf:return selfelif np.all(np.abs(indices) < 2):  # depth is 1child_node = self.get_child_node(indices)if child_node is None:return Noneelse:return child_nodeelse:  # depth is 1voxel_indices = self.voxel_indices * 2 + indicesreturn self.parent_node.get_voxel(voxel_indices, depth_from_this_node + 1)def get_child_node(self, indices):child_index = indices[0] * 4 + indices[1] * 2 + indices[2]return self.child_nodes[child_index]def get_indices_from_root(self):if self.parent_node is None:return self.voxel_indiceselse:return self.voxel_indices + 2 * self.parent_node.get_indices_from_root()def get_child_center_points(self):if self.is_leaf:return [self.center]else:points = []for child_node in self.child_nodes:if child_node is not None:points.extend(child_node.get_child_center_points())return pointsdef get_all_voxels(self):if self.is_leaf:return [self.center], [self]else:centers, voxels = [], [], []for child_node in self.child_nodes:if child_node is not None:child_centers, child_voxels = child_node.get_all_voxels()centers.extend(child_centers)voxels.extend(child_voxels)return centers, voxelsdef visualize(self):points = self.get_child_center_points()pcd = o3d.geometry.PointCloud()pcd.points = o3d.utility.Vector3dVector(np.vstack(points))o3d.visualization.draw_geometries([pcd])def build_child_tree(self, points):self.num_points = points.shape[0]# print("Number of points", self.num_points, self.min_values, self.max_values)self.center = (self.max_values + self.min_values) * 0.5half_sizes = (self.max_values - self.min_values) * 0.5if self.empty():return Falseif np.any(half_sizes < self.min_voxel_sizes):self.points = pointsself.is_leaf = Truereturn Trueelse:points_3d = pointsfor i in range(2):for j in range(2):for k in range(2):child_node = OccupancyVoxelTree(self.min_voxel_sizes, self, self.depth + 1)child_node.voxel_indices = np.array([i, j, k])child_node.min_values = self.min_values + child_node.voxel_indices * half_sizeschild_node.max_values = child_node.min_values + half_sizespoint_indices = np.where(np.all(points_3d >= child_node.min_values, axis=1) &np.all(points_3d < child_node.max_values, axis=1))[0]if child_node.build_child_tree(points[point_indices, :]):self.child_nodes.append(child_node)else:self.child_nodes.append(None)return True

3. 开源库

PCL八叉树组件提供了几种八叉树类型。它们的主要区别在于它们的单个叶节点特征。

OctreePointCloudPointVector(等于OctreePointCloud):这个八叉树可以在每个叶节点上保存一个点索引列表。
OctreePointCloudSinglePoint:这个八叉树类在每个叶节点上只有一个单点索引。仅存储分配给叶节点的最近的点索引。
OctreePointCloudOccupancy:这个八叉树在它的叶子节点上不存储任何点信息。它可以用于空间占用检查。
OctreePointCloudDensity:这个八叉树计算每个叶节点体素内的点的数量。它允许空间密度查询。
如果需要高速创建八叉树，请查看八叉树双缓冲实现(Octree2BufBase类)。这个类同时在内存中保存两个并行的八叉树结构。除了搜索操作之外，这还支持空间变化检测。此外，高级内存管理减少了八叉树构建过程中的内存分配和回收操作。双缓冲八叉树实现可以通过模板参数OctreeT赋值给所有的OctreePointCloud类。
所有八叉树都支持八叉树结构和八叉树数据内容的序列化和非序列化。

 #include <pcl/point_cloud.h>#include <pcl/octree/octree_search.h>#include <iostream>#include <vector>#include <ctime>intmain (){srand ((unsigned int) time (NULL));// [1]创建点云指针pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);// [2] 构造1000个随机的点云数据cloud->width = 1000;cloud->height = 1;cloud->points.resize (cloud->width * cloud->height);for (std::size_t i = 0; i < cloud->size (); ++i){(*cloud)[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);(*cloud)[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);(*cloud)[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);}float resolution = 128.0f;// [3]八叉树点云搜索实例pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);// [4]将点云设置成八叉树结构octree.setInputCloud (cloud);octree.addPointsFromInputCloud ();// [5]随机定义一个要查找的点pcl::PointXYZ searchPoint;searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);//**************************体素搜索（搜索相同体素内的点）************************// [6]创建点云索引容器std::vector<int> pointIdxVec;// [7]开始进行体素内近邻点搜索if (octree.voxelSearch (searchPoint, pointIdxVec)){// [8] 将查找点所在体素内的邻点全都输出打印到命令窗口std::cout << "Neighbors within voxel search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl;for (std::size_t i = 0; i < pointIdxVec.size (); ++i)std::cout << "    " << (*cloud)[pointIdxVec[i]].x << " " << (*cloud)[pointIdxVec[i]].y << " " << (*cloud)[pointIdxVec[i]].z << std::endl;}//*************************K近邻搜索（搜索K个最近点）************************int K = 10;// 创建点索引容器，用于按距离保存搜索到的点std::vector<int> pointIdxNKNSearch;// 创建点距离容器 std::vector<float> pointNKNSquaredDistance;std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z<< ") with K=" << K << std::endl;// 开始最近K邻搜索if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){for (std::size_t i = 0; i < pointIdxNKNSearch.size (); ++i)std::cout << "    "  <<   (*cloud)[ pointIdxNKNSearch[i] ].x << " " << (*cloud)[ pointIdxNKNSearch[i] ].y << " " << (*cloud)[ pointIdxNKNSearch[i] ].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;}//*************************K近邻搜索（搜索半径范围内的点）************************// 创建半径范围内点索引的容器std::vector<int> pointIdxRadiusSearch;// 创建搜索到的点的距离容器std::vector<float> pointRadiusSquaredDistance;float radius = 256.0f * rand () / (RAND_MAX + 1.0f);std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z<< ") with radius=" << radius << std::endl;// 开始半径搜索if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0){for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)std::cout << "    "  <<   (*cloud)[ pointIdxRadiusSearch[i] ].x << " " << (*cloud)[ pointIdxRadiusSearch[i] ].y << " " << (*cloud)[ pointIdxRadiusSearch[i] ].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;}}