KD Tree#

kd_tree_feature_matching.py#

 1# ----------------------------------------------------------------------------
 2# -                        CloudViewer: www.cloudViewer.org                  -
 3# ----------------------------------------------------------------------------
 4# Copyright (c) 2018-2024 www.cloudViewer.org
 5# SPDX-License-Identifier: MIT
 6# ----------------------------------------------------------------------------
 7
 8import numpy as np
 9import cloudViewer as cv3d
10
11if __name__ == "__main__":
12
13    print("Load two aligned point clouds.")
14    demo_data = cv3d.data.DemoFeatureMatchingPointClouds()
15    pcd0 = cv3d.io.read_point_cloud(demo_data.point_cloud_paths[0])
16    pcd1 = cv3d.io.read_point_cloud(demo_data.point_cloud_paths[1])
17
18    pcd0.paint_uniform_color([1, 0.706, 0])
19    pcd1.paint_uniform_color([0, 0.651, 0.929])
20    cv3d.visualization.draw_geometries([pcd0, pcd1])
21    print("Load their FPFH feature and evaluate.")
22    print("Black : matching distance > 0.2")
23    print("White : matching distance = 0")
24    feature0 = cv3d.io.read_feature(demo_data.fpfh_feature_paths[0])
25    feature1 = cv3d.io.read_feature(demo_data.fpfh_feature_paths[1])
26
27    fpfh_tree = cv3d.geometry.KDTreeFlann(feature1)
28    for i in range(len(pcd0.points())):
29        [_, idx, _] = fpfh_tree.search_knn_vector_xd(feature0.data[:, i], 1)
30        dis = np.linalg.norm(pcd0.point(i) - pcd1.point(idx[0]))
31        c = (0.2 - np.fmin(dis, 0.2)) / 0.2
32        pcd0.set_color(i, [c, c, c])
33    cv3d.visualization.draw_geometries([pcd0])
34    print("")
35
36    print("Load their L32D feature and evaluate.")
37    print("Black : matching distance > 0.2")
38    print("White : matching distance = 0")
39    feature0 = cv3d.io.read_feature(demo_data.l32d_feature_paths[0])
40    feature1 = cv3d.io.read_feature(demo_data.l32d_feature_paths[1])
41
42    fpfh_tree = cv3d.geometry.KDTreeFlann(feature1)
43    for i in range(len(pcd0.points())):
44        [_, idx, _] = fpfh_tree.search_knn_vector_xd(feature0.data[:, i], 1)
45        dis = np.linalg.norm(pcd0.point(i) - pcd1.point(idx[0]))
46        c = (0.2 - np.fmin(dis, 0.2)) / 0.2
47        pcd0.set_color(i, [c, c, c])
48    cv3d.visualization.draw_geometries([pcd0])
49    print("")

kd_tree_search.py#

 1# ----------------------------------------------------------------------------
 2# -                        CloudViewer: www.cloudViewer.org                  -
 3# ----------------------------------------------------------------------------
 4# Copyright (c) 2018-2024 www.cloudViewer.org
 5# SPDX-License-Identifier: MIT
 6# ----------------------------------------------------------------------------
 7"""Build a KDTree and use it for neighbour search"""
 8
 9import numpy as np
10import cloudViewer as cv3d
11
12if __name__ == "__main__":
13    print("Testing kdtree in cloudViewer ...")
14    print("Load a point cloud and paint it gray.")
15    sample_pcd_data = cv3d.data.PCDPointCloud()
16    pcd = cv3d.io.read_point_cloud(sample_pcd_data.path)
17    print(pcd)
18    pcd.paint_uniform_color([0.5, 0.5, 0.5])
19    pcd_tree = cv3d.geometry.KDTreeFlann(pcd)
20
21    print("Paint the 1500th point red.")
22    pcd.set_color(1500, [1, 0, 0])
23
24    print("Find its 200 nearest neighbors, paint blue.")
25    [k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.get_point(1500), 200)
26    colors = np.asarray(pcd.get_colors())
27    colors[idx[1:], :] = [0, 0, 1]
28    pcd.set_colors(cv3d.utility.Vector3dVector(colors))
29
30    print("Find its neighbors with distance less than 0.2, paint green.")
31    [k, idx, _] = pcd_tree.search_radius_vector_3d(pcd.get_point(1500), 0.2)
32    colors = np.asarray(pcd.get_colors())
33    colors[idx[1:], :] = [0, 1, 0]
34    pcd.set_colors(cv3d.utility.Vector3dVector(colors))
35
36    print("Visualize the point cloud.")
37    cv3d.visualization.draw_geometries([pcd],
38                                       zoom=0.5599,
39                                       front=[-0.4958, 0.8229, 0.2773],
40                                       lookat=[2.1126, 1.0163, -1.8543],
41                                       up=[0.1007, -0.2626, 0.9596])
42    print("")