测试环境：
pcl==1.12.1
python-pcl==0.3.1
python==3.7
代码：
# -*- coding: utf-8 -*-
# Spatial Partitioning and Search Operations with Octrees
# http://pointclouds.org/documentation/tutorials/octree.php#octree-search
import pcl
import numpy as np
import random
def main():
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud = pcl.PointCloud()
##
# // Generate pointcloud data
# cloud->width = 1000;
# cloud->height = 1;
# cloud->points.resize (cloud->width * cloud->height);
#
# for (size_t i = 0; i < cloud->points.size (); ++i)
# {
# cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
# cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
# cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
# }
#
points = np.zeros((1000, 3), dtype=np.float32)
RAND_MAX = 1024.0
for i in range(0, 1000):
points[i][0] = 1024 * random.random() / RAND_MAX
points[i][1] = 1024 * random.random() / RAND_MAX
points[i][2] = 1024 * random.random() / RAND_MAX
cloud.from_array(points)
# pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
# octree.setInputCloud (cloud);
# octree.addPointsFromInputCloud ();
# resolution = 128.0
# x,y,z Area Filter
resolution = 0.2
octree = cloud.make_octreeSearch(resolution)
octree.add_points_from_input_cloud()
# 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);
searchPoint = pcl.PointCloud()
searchPoints = np.zeros((1, 3), dtype=np.float32)
# searchPoints[0][0] = 1024 * random.random () / (RAND_MAX + 1.0)
# searchPoints[0][1] = 1024 * random.random () / (RAND_MAX + 1.0)
# searchPoints[0][2] = 1024 * random.random () / (RAND_MAX + 1.0)
searchPoints[0][0] = 1024 * random.random() / (RAND_MAX + 1.0)
searchPoints[0][1] = 1024 * random.random() / (RAND_MAX + 1.0)
searchPoints[0][2] = 1024 * random.random() / (RAND_MAX + 1.0)
searchPoint.from_array(searchPoints)
##
# // Neighbors within voxel search
# std::vector<int> pointIdxVec;
#
# if (octree.voxelSearch (searchPoint, pointIdxVec))
# {
# std::cout << "Neighbors within voxel search at (" << searchPoint.x
# << " " << searchPoint.y
# << " " << searchPoint.z << ")"
# << std::endl;
#
# for (size_t i = 0; i < pointIdxVec.size (); ++i)
# std::cout << " " << cloud->points[pointIdxVec[i]].x
# << " " << cloud->points[pointIdxVec[i]].y
# << " " << cloud->points[pointIdxVec[i]].z << std::endl;
# }
ind = octree.VoxelSearch(searchPoint)
print('Neighbors within voxel search at (' + str(searchPoint[0][0]) + ' ' + str(
searchPoint[0][1]) + ' ' + str(searchPoint[0][2]) + ')')
# for i in range(0, ind.size):
for i in range(0, ind.size):
print('index = ' + str(ind[i]))
print('(' + str(cloud[ind[i]][0]) + ' ' +
str(cloud[ind[i]][1]) + ' ' + str(cloud[ind[i]][2]))
##
# // K nearest neighbor search
# 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 = 10
print('K nearest neighbor search at (' + str(searchPoint[0][0]) + ' ' + str(
searchPoint[0][1]) + ' ' + str(searchPoint[0][2]) + ') with K=' + str(K))
# if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
# {
# for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
# std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x
# << " " << cloud->points[ pointIdxNKNSearch[i] ].y
# << " " << cloud->points[ pointIdxNKNSearch[i] ].z
# << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
# }
# // Neighbors within radius search
[ind, sqdist] = octree.nearest_k_search_for_cloud(searchPoint, K)
# if nearest_k_search_for_cloud
for i in range(0, ind.size):
print('(' + str(cloud[ind[0][i]][0]) + ' ' + str(cloud[ind[0][i]][1]) + ' ' + str(
cloud[ind[0][i]][2]) + ' (squared distance: ' + str(sqdist[0][i]) + ')')
##
# 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;
#
radius = 256.0 * random.random() / (RAND_MAX + 1.0)
print('Neighbors within radius search at (' + str(searchPoint[0][0]) + ' ' + str(
searchPoint[0][1]) + ' ' + str(searchPoint[0][2]) + ') with radius=' + str(radius))
# if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
# {
# for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
# std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
# << " " << cloud->points[ pointIdxRadiusSearch[i] ].y
# << " " << cloud->points[ pointIdxRadiusSearch[i] ].z
# << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
# }
###
# [ind, sqdist] = octree.radius_search_for_cloud (searchPoint, radius)
# Exception ignored in: 'pcl._pcl.to_point_t'
# [ind, sqdist] = octree.radius_search (searchPoint, radius, 10)
searchPoints = (searchPoint[0][0], searchPoint[0][1], searchPoint[0][2])
[ind, sqdist] = octree.radius_search(searchPoints, radius, 10)
# Function radius_search
for i in range(0, ind.size):
print('(' + str(cloud[ind[i]][0]) + ' ' + str(cloud[ind[i]][1]) + ' ' +
str(cloud[ind[i]][2]) + ' (squared distance: ' + str(sqdist[i]) + ')')
# Function radius_search_for_cloud
# for i in range(0, ind.size):
# print ('(' + str(cloud[ind[0][i]][0]) + ' ' + str(cloud[ind[0][i]][1]) + ' ' + str(cloud[ind[0][i]][2]) + ' (squared distance: ' + str(sqdist[0][i]) + ')')
if __name__ == "__main__":
# import cProfile
# cProfile.run('main()', sort='time')
main()运行结果：
VoxelSearch at (0.37849587202072144 0.19727467000484467 0.3064234256744385)
Neighbors within voxel search at (0.37849587202072144 0.19727467000484467 0.3064234256744385)
index = 45
(0.4282791316509247 0.19563999772071838 0.22944234311580658
index = 364
(0.44496798515319824 0.04221676290035248 0.3575461804866791
index = 388
(0.48459964990615845 0.1802610605955124 0.2799946069717407
index = 907
(0.458575040102005 0.1072542816400528 0.24344711005687714
K nearest neighbor search at (0.37849587202072144 0.19727467000484467 0.3064234256744385) with K=10
(0.335491806268692 0.1623605340719223 0.26550593972206116 (squared distance: 0.0047425875)
(0.3500078320503235 0.19176329672336578 0.38017719984054565 (squared distance: 0.0062815626)
(0.30588388442993164 0.1718531847000122 0.34713444113731384 (squared distance: 0.0075761396)
(0.4282791316509247 0.19563999772071838 0.22944234311580658 (squared distance: 0.008407132)
(0.31789419054985046 0.1155492439866066 0.28539803624153137 (squared distance: 0.010793676)
(0.43619975447654724 0.267613023519516 0.3614135682582855 (squared distance: 0.0113011375)
(0.3559480905532837 0.2263394445180893 0.20286111533641815 (squared distance: 0.012078315)
(0.48459964990615845 0.1802610605955124 0.2799946069717407 (squared distance: 0.012245957)
(0.2636048495769501 0.16589123010635376 0.29033729434013367 (squared distance: 0.01444363)
(0.458575040102005 0.1072542816400528 0.24344711005687714 (squared distance: 0.018482361)
Neighbors within radius search at (0.37849587202072144 0.19727467000484467 0.3064234256744385) with radius=0.14303657495545388
(0.3559480905532837 0.2263394445180893 0.20286111533641815 (squared distance: 0.012078315)
(0.2636048495769501 0.16589123010635376 0.29033729434013367 (squared distance: 0.01444363)
(0.3500078320503235 0.19176329672336578 0.38017719984054565 (squared distance: 0.0062815626)
(0.30588388442993164 0.1718531847000122 0.34713444113731384 (squared distance: 0.0075761396)
(0.335491806268692 0.1623605340719223 0.26550593972206116 (squared distance: 0.0047425875)
(0.31789419054985046 0.1155492439866066 0.28539803624153137 (squared distance: 0.010793676)
(0.4282791316509247 0.19563999772071838 0.22944234311580658 (squared distance: 0.008407132)
(0.48459964990615845 0.1802610605955124 0.2799946069717407 (squared distance: 0.012245957)
(0.458575040102005 0.1072542816400528 0.24344711005687714 (squared distance: 0.018482361)
(0.43619975447654724 0.267613023519516 0.3614135682582855 (squared distance: 0.0113011375)
