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

# -*- coding: utf-8 -*-
from __future__ import print_function

import numpy as np
import pcl


def 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()