前言本教程代碼開源：GitHub 歡迎star
八叉樹是一種基于樹的數據結構，用于管理稀疏 3-D 數據。每個內部節點正好有八個子節點。在本教程中，我們將學習如何使用八叉樹在點云數據中進行空間分區和鄰居搜索。特別地，我們解釋了如何執行“體素內的鄰居搜索”、“K 最近鄰搜索”和“半徑內的鄰居搜索”。
代碼import pclpyfrom pclpy import pclimport numpy as npif __name__ == __main__: # 生成點云數據 cloud_size = 1000 a = np.random.ranf(cloud_size * 3).reshape(-1, 3) * 1024 cloud = pcl.PointCloud.PointXYZ.from_array(a) resolution = 128.0 octree = pcl.octree.OctreePointCloudSearch.PointXYZ(resolution) octree.setInputCloud(cloud) octree.addPointsFromInputCloud() searchPoint = pcl.point_types.PointXYZ() searchPoint.x = np.random.ranf(1) * 1024 searchPoint.y = np.random.ranf(1) * 1024 searchPoint.z = np.random.ranf(1) * 1024 # 體素最近鄰搜索 pointIdxVec = pclpy.pcl.vectors.Int() if octree.voxelSearch(searchPoint, pointIdxVec) > 0: print(Neighbors within voxel search at (, searchPoint.x, , searchPoint.y, , searchPoint.z, )/n) for i in range(len(pointIdxVec)): print(" ", cloud.x[pointIdxVec[i]], " ", cloud.y[pointIdxVec[i]], " ", cloud.z[pointIdxVec[i]], "/n") # k最近鄰搜索 k = 10 pointIdxNKNSearch = pclpy.pcl.vectors.Int() pointNKNSquaredDistance = pclpy.pcl.vectors.Float() print(K nearest neighbor search at (, searchPoint.x, , searchPoint.y, , searchPoint.z, ) with k =, k, /n) if octree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0: for i in range(len(pointIdxNKNSearch)): print(" ", cloud.x[pointIdxNKNSearch[i]], " ", cloud.y[pointIdxNKNSearch[i]], " ", cloud.z[pointIdxNKNSearch[i]], " (squared distance: ", pointNKNSquaredDistance[i], ")", "/n") # 半徑最近鄰搜索 pointIdxRadiusSearch = pclpy.pcl.vectors.Int() pointRadiusSquaredDistance = pclpy.pcl.vectors.Float() radius = np.random.ranf(1) * 256.0 print("Neighbors within radius search at (", searchPoint.x, " ", searchPoint.y, " ", searchPoint.z, ") with radius=", radius, /n) if octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0: for i in range(len(pointIdxRadiusSearch)): print(" ", cloud.x[pointIdxRadiusSearch[i]], " ", cloud.y[pointIdxRadiusSearch[i]], " ", cloud.z[pointIdxRadiusSearch[i]], " (squared distance: ", pointRadiusSquaredDistance[i], ")", "/n")首先定義并實例化一個共享的 PointCloud 結構，并用隨機點填充它。
# 生成點云數據cloud_size = 1000a = np.random.ranf(cloud_size * 3).reshape(-1, 3) * 1024cloud = pcl.PointCloud.PointXYZ.from_array(a)然后我們創建一個八叉樹實例，用它的分辨率初始化。該八叉樹在其葉節點內保留了一個點索引向量。分辨率參數描述了最低八叉樹級別的最小體素的長度。因此，八叉樹的深度是點云的分辨率和空間維度的函數。如果知道點云的邊界框，則應使用defineBoundingBox 方法將其分配給八叉樹。然后我們分配一個指向 PointCloud 的指針并將所有點添加到八叉樹。
resolution = 128.0octree = pcl.octree.OctreePointCloudSearch.PointXYZ(resolution)octree.setInputCloud(cloud)octree.addPointsFromInputCloud()一旦 PointCloud 與八叉樹相關聯，我們就可以執行搜索操作。這里使用的第一種搜索方法是“體素內的鄰居搜索”。它將搜索點分配給相應的葉節點體素并返回點索引向量。這些指數與落在同一體素內的點有關。因此，搜索點和搜索結果之間的距離取決于八叉樹的分辨率參數。
# 體素最近鄰搜索pointIdxVec = pclpy.pcl.vectors.Int()if octree.voxelSearch(searchPoint, pointIdxVec) > 0: print(Neighbors within voxel search at (, searchPoint.x, , searchPoint.y, , searchPoint.z, )/n) for i in range(len(pointIdxVec)): print(" ", cloud.x[pointIdxVec[i]], " ", cloud.y[pointIdxVec[i]], " ", cloud.z[pointIdxVec[i]], "/n")接下來，演示 K 最近鄰搜索。在此示例中，K 設置為 10。“K 最近鄰搜索”方法將搜索結果寫入兩個多帶帶的向量。第一個 pointIdxNKNSearch 將包含搜索結果（索引引用關聯的 PointCloud 數據集）。第二個向量保存搜索點和最近鄰居之間的相應平方距離。
# k最近鄰搜索k = 10pointIdxNKNSearch = pclpy.pcl.vectors.Int()pointNKNSquaredDistance = pclpy.pcl.vectors.Float()print(K nearest neighbor search at (, searchPoint.x, , searchPoint.y, , searchPoint.z, ) with k =, k, /n)if octree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0: for i in range(len(pointIdxNKNSearch)): print(" ", cloud.x[pointIdxNKNSearch[i]], " ", cloud.y[pointIdxNKNSearch[i]], " ", cloud.z[pointIdxNKNSearch[i]], " (squared distance: ", pointNKNSquaredDistance[i], ")", "/n")“半徑搜索中的鄰居”的工作方式與“K 最近鄰搜索”非常相似。它的搜索結果被寫入兩個多帶帶的向量，描述點索引和平方搜索點距離。
# 半徑最近鄰搜索pointIdxRadiusSearch = pclpy.pcl.vectors.Int()pointRadiusSquaredDistance = pclpy.pcl.vectors.Float()radius = np.random.ranf(1) * 256.0print("Neighbors within radius search at (", searchPoint.x, " ", searchPoint.y, " ", searchPoint.z, ") with radius=", radius, /n)if octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0: for i in range(len(pointIdxRadiusSearch)): print(" ", cloud.x[pointIdxRadiusSearch[i]], " ", cloud.y[pointIdxRadiusSearch[i]], " ", cloud.z[pointIdxRadiusSearch[i]], " (squared distance: ", pointRadiusSquaredDistance[i], ")", "/n")運行02_octree_search.py
運行結果：
Neighbors within voxel search at ( 235.9908905029297 550.4008178710938 457.9418029785156 )
284.7338 645.86816 439.52136
K nearest neighbor search at ( 235.9908905029297 550.4008178710938 457.9418029785156 ) with k = 10
270.4592 528.7933 455.80542 (squared distance: 1659.5142822265625 )
179.18915 504.20984 424.5981 (squared distance: 6471.84619140625 )
219.09846 518.7366 371.57288 (squared distance: 8747.5703125 )
282.65546 499.14508 375.4459 (squared distance: 11610.3076171875 )
284.7338 645.86816 439.52136 (squared distance: 11829.1982421875 )
152.6541 517.1704 523.69183 (squared distance: 12372.34765625 )
222.60712 437.74167 441.51077 (squared distance: 13141.1875 )
283.5196 504.1384 557.749 (squared distance: 14360.6708984375 )
236.29802 493.2118 336.7504 (squared distance: 17958.03515625 )
213.21176 569.0312 589.7377 (squared distance: 18236.12890625 )
Neighbors within radius search at ( 235.9908905029297 550.4008178710938 457.9418029785156 ) with radius= [108.47992978]
179.18915 504.20984 424.5981 (squared distance: 6471.84619140625 )
219.09846 518.7366 371.57288 (squared distance: 8747.5703125 )
282.65546 499.14508 375.4459 (squared distance: 11610.3076171875 )
270.4592 528.7933 455.80542 (squared distance: 1659.5142822265625 )
注意：由于我們的數據是隨生成的，每次結果都不一樣，甚至有時候可能KdTree 返回 0 最近鄰，這時候就沒有輸出了。可以適當增大cloud_size，效果會更明顯。
其他PCL 八叉樹組件提供了幾種八叉樹類型。它們的基本區別在于它們各自的葉節點特征。
- OctreePointCloudPointVector（等于 OctreePointCloud）：這個八叉樹可以在每個葉節點上保存一個點索引列表。
- OctreePointCloudSinglePoint：這個八叉樹類在每個葉節點上只保存一個點索引。僅存儲分配給葉節點的最新點索引。
- OctreePointCloudOccupancy：這個八叉樹在其葉節點不存儲任何點信息。它可用于空間占用檢查。
- OctreePointCloudDensity：這個八叉樹計算每個葉節點體素內的點數。它允許空間密度查詢。
如果需要高速創建八叉樹，請查看八叉樹雙緩沖實現（Octree2BufBase 類）。這個類同時在內存中保持兩個并行的八叉樹結構。除了搜索操作外，這還可以實現空間變化檢測。此外，先進的內存管理減少了八叉樹構建過程中的內存分配和釋放操作。雙緩沖八叉樹實現可以通過模板參數“OctreeT”分配給所有 OctreePointCloud 類。
所有八叉樹都支持八叉樹結構和八叉樹數據內容的序列化和反序列化。
總結PCL 八叉樹實現是空間分區和搜索操作的強大工具。
