本教程代碼開源：GitHub 歡迎star

前言

八叉樹是一種基于樹的數(shù)據(jù)結(jié)構(gòu)，用于管理稀疏 3-D 數(shù)據(jù)。每個內(nèi)部節(jié)點正好有八個子節(jié)點。在本教程中，我們將學(xué)習(xí)如何使用八叉樹在點云數(shù)據(jù)中進行空間分區(qū)和鄰居搜索。特別地，我們解釋了如何執(zhí)行“體素內(nèi)的鄰居搜索”、“K 最近鄰搜索”和“半徑內(nèi)的鄰居搜索”。

代碼

見02_octree_search.py

import pclpyfrom pclpy import pclimport numpy as npif __name__ == __main__:    # 生成點云數(shù)據(jù)    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 結(jié)構(gòu)，并用隨機點填充它。

# 生成點云數(shù)據(jù)cloud_size = 1000a = np.random.ranf(cloud_size * 3).reshape(-1, 3) * 1024cloud = pcl.PointCloud.PointXYZ.from_array(a)

然后我們創(chuàng)建一個八叉樹實例，用它的分辨率初始化。該八叉樹在其葉節(jié)點內(nèi)保留了一個點索引向量。分辨率參數(shù)描述了最低八叉樹級別的最小體素的長度。因此，八叉樹的深度是點云的分辨率和空間維度的函數(shù)。如果知道點云的邊界框，則應(yīng)使用defineBoundingBox 方法將其分配給八叉樹。然后我們分配一個指向 PointCloud 的指針并將所有點添加到八叉樹。

resolution = 128.0octree = pcl.octree.OctreePointCloudSearch.PointXYZ(resolution)octree.setInputCloud(cloud)octree.addPointsFromInputCloud()

一旦 PointCloud 與八叉樹相關(guān)聯(lián)，我們就可以執(zhí)行搜索操作。這里使用的第一種搜索方法是“體素內(nèi)的鄰居搜索”。它將搜索點分配給相應(yīng)的葉節(jié)點體素并返回點索引向量。這些指數(shù)與落在同一體素內(nèi)的點有關(guān)。因此，搜索點和搜索結(jié)果之間的距離取決于八叉樹的分辨率參數(shù)。

# 體素最近鄰搜索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 設(shè)置為 10?！癒 最近鄰搜索”方法將搜索結(jié)果寫入兩個多帶帶的向量。第一個 pointIdxNKNSearch 將包含搜索結(jié)果（索引引用關(guān)聯(lián)的 PointCloud 數(shù)據(jù)集）。第二個向量保存搜索點和最近鄰居之間的相應(yīng)平方距離。

# 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 最近鄰搜索”非常相似。它的搜索結(jié)果被寫入兩個多帶帶的向量，描述點索引和平方搜索點距離。

# 半徑最近鄰搜索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

運行結(jié)果：

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 )

注意：由于我們的數(shù)據(jù)是隨生成的，每次結(jié)果都不一樣，甚至有時候可能KdTree 返回 0 最近鄰，這時候就沒有輸出了。可以適當增大cloud_size，效果會更明顯。

其他

PCL 八叉樹組件提供了幾種八叉樹類型。它們的基本區(qū)別在于它們各自的葉節(jié)點特征。

  • OctreePointCloudPointVector（等于 OctreePointCloud）：這個八叉樹可以在每個葉節(jié)點上保存一個點索引列表。
  • OctreePointCloudSinglePoint：這個八叉樹類在每個葉節(jié)點上只保存一個點索引。僅存儲分配給葉節(jié)點的最新點索引。
  • OctreePointCloudOccupancy：這個八叉樹在其葉節(jié)點不存儲任何點信息。它可用于空間占用檢查。
  • OctreePointCloudDensity：這個八叉樹計算每個葉節(jié)點體素內(nèi)的點數(shù)。它允許空間密度查詢。

如果需要高速創(chuàng)建八叉樹，請查看八叉樹雙緩沖實現(xiàn)（Octree2BufBase 類）。這個類同時在內(nèi)存中保持兩個并行的八叉樹結(jié)構(gòu)。除了搜索操作外，這還可以實現(xiàn)空間變化檢測。此外，先進的內(nèi)存管理減少了八叉樹構(gòu)建過程中的內(nèi)存分配和釋放操作。雙緩沖八叉樹實現(xiàn)可以通過模板參數(shù)“OctreeT”分配給所有 OctreePointCloud 類。

所有八叉樹都支持八叉樹結(jié)構(gòu)和八叉樹數(shù)據(jù)內(nèi)容的序列化和反序列化。

總結(jié)

PCL 八叉樹實現(xiàn)是空間分區(qū)和搜索操作的強大工具。