File: kdtree.py

package info (click to toggle)
python-pcl 0.3.0~rc1%2Bdfsg-14
  • links: PTS, VCS
  • area: main
  • in suites: bookworm
  • size: 31,828 kB
  • sloc: python: 3,094; cpp: 283; makefile: 181; sh: 24; ansic: 12
file content (40 lines) | stat: -rw-r--r-- 1,079 bytes parent folder | download | duplicates (3)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
# -*- coding: utf-8 -*-

from __future__ import print_function

import numpy as np
import pcl

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])