This tutorial is open source: GitHub Welcome, star
preface
Octree is a tree based data structure used to manage sparse 3-D data. Each internal node has exactly eight child nodes. In this tutorial, we will learn how to use octree for spatial partition and neighbor search in point cloud data. In particular, we explained how to perform "neighbor search within voxel", "K-nearest neighbor search" and "neighbor search within radius".
code
import pclpy
from pclpy import pcl
import numpy as np
if __name__ == '__main__':
# Generate point cloud data
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
# Voxel nearest neighbor search
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 nearest neighbor search
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")
# Radius nearest neighbor search
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")
explain
First, define and instantiate a shared PointCloud structure and fill it with random points.
# Generate point cloud data cloud_size = 1000 a = np.random.ranf(cloud_size * 3).reshape(-1, 3) * 1024 cloud = pcl.PointCloud.PointXYZ.from_array(a)
Then we create an octree instance and initialize it with its resolution. The octree retains a point index vector in its leaf nodes. The resolution parameter describes the length of the smallest voxel at the lowest octree level. Therefore, the depth of the octree is a function of the resolution and spatial dimension of the point cloud. If you know the bounding box of the point cloud, you should assign it to the octree using the defineBoundingBox method. Then we assign a pointer to PointCloud and add all points to the octree.
resolution = 128.0 octree = pcl.octree.OctreePointCloudSearch.PointXYZ(resolution) octree.setInputCloud(cloud) octree.addPointsFromInputCloud()
Once the PointCloud is associated with the octree, we can perform the search operation. The first search method used here is "neighbor search within voxels". It assigns the search point to the corresponding leaf node voxel and returns the point index vector. These indices are related to points that fall within the same entity. Therefore, the distance between the search point and the search result depends on the resolution parameter of the octree.
# Voxel nearest neighbor search
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")
Next, we demonstrate k-nearest neighbor search. In this example, K is set to 10. The k-nearest neighbor search method writes the search results to two separate vectors. The first pointIdxNKNSearch will contain the search results (the index references the associated PointCloud dataset). The second vector holds the corresponding square distance between the search point and the nearest neighbor.
# k nearest neighbor search
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")
Neighbors in radius search works very similar to K-nearest neighbor search. Its search results are written into two separate vectors, the description point index and the square search point distance.
# Radius nearest neighbor search
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")
function
Run 02_octree_search.py
Operation results:
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 )
Note: because our data is generated with, the results are different every time. Sometimes, KdTree may return the nearest neighbor of 0, and there is no output at this time. You can increase the cloud size appropriately_ Size, the effect will be more obvious.
other
The PCL octree component provides several octree types. Their basic difference lies in their respective leaf node characteristics.
- OctreePointCloudPointVector (equal to OctreePointCloud): This octree can save a point index list on each leaf node.
- OctreePointCloudSinglePoint: This octree class saves only one point index on each leaf node. Stores only the latest point index assigned to the leaf node.
- Octree point cloud occupancy: This octree does not store any point information in its leaf nodes. It can be used for space occupancy checks.
- OctreePointCloudDensity: This octree calculates the number of points in each leaf node voxel. It allows spatial density queries.
If you need to create an octree at a high speed, please check the octree double buffering implementation (Octree2BufBase class). This class maintains two parallel octree structures in memory at the same time. In addition to the search operation, this can also realize spatial change detection. In addition, advanced memory management reduces memory allocation and release operations during octree construction. The double buffered octree implementation can be assigned to all OctreePointCloud classes through the template parameter "OctreeT".
All octrees support octree structure and serialization and deserialization of octree data content.
summary
The implementation of PCL octree is a powerful tool for spatial partition and search operation.