[python][pcl]python-pcl案例之kdtree使用

测试环境：

pcl==1.13.0

python-pcl==0.3.1

python==3.7

代码：

# -*- coding: utf-8 -*-
from __future__ import print_functionimport numpy as np
import pcldef main():points_1 = np.array([[0, 0, 0],[1, 0, 0],[0, 1, 0],[1, 1, 0]], dtype=np.float32)points_2 = np.array([[0, 0, 0.2],[1, 0, 0],[0, 1, 0],[1.1, 1, 0.5]], dtype=np.float32)pc_1 = pcl.PointCloud()pc_1.from_array(points_1)pc_2 = pcl.PointCloud()pc_2.from_array(points_2)kd = pcl.KdTreeFLANN(pc_1)print('pc_1:')print(points_1)print('\\npc_2:')print(points_2)print('\\n')pc_1 = pcl.PointCloud(points_1)pc_2 = pcl.PointCloud(points_2)kd = pc_1.make_kdtree_flann()# find the single closest points to each point in point cloud 2# (and the sqr distances)indices, sqr_distances = kd.nearest_k_search_for_cloud(pc_2, 1)for i in range(pc_1.size):print('index of the closest point in pc_1 to point %d in pc_2 is %d'% (i, indices[i, 0]))print('the squared distance between these two points is %f'% sqr_distances[i, 0])if __name__ == "__main__":# import cProfile# cProfile.run('main()', sort='time')main()

输出：

pc_1:
[[0. 0. 0.]
 [1. 0. 0.]
 [0. 1. 0.]
 [1. 1. 0.]]

pc_2:
[[0.  0.  0.2]
 [1.  0.  0. ]
 [0.  1.  0. ]
 [1.1 1.  0.5]]

index of the closest point in pc_1 to point 0 in pc_2 is 0
the squared distance between these two points is 0.040000
index of the closest point in pc_1 to point 1 in pc_2 is 1
the squared distance between these two points is 0.000000
index of the closest point in pc_1 to point 2 in pc_2 is 2
the squared distance between these two points is 0.000000
index of the closest point in pc_1 to point 3 in pc_2 is 3
the squared distance between these two points is 0.260000