# KD Tree¶

## kd_tree_feature_matching.py¶

  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 41 42 43 44 45 46 47 48 49 # ---------------------------------------------------------------------------- # - Open3D: www.open3d.org - # ---------------------------------------------------------------------------- # Copyright (c) 2018-2023 www.open3d.org # SPDX-License-Identifier: MIT # ---------------------------------------------------------------------------- import numpy as np import open3d as o3d if __name__ == "__main__": print("Load two aligned point clouds.") demo_data = o3d.data.DemoFeatureMatchingPointClouds() pcd0 = o3d.io.read_point_cloud(demo_data.point_cloud_paths[0]) pcd1 = o3d.io.read_point_cloud(demo_data.point_cloud_paths[1]) pcd0.paint_uniform_color([1, 0.706, 0]) pcd1.paint_uniform_color([0, 0.651, 0.929]) o3d.visualization.draw_geometries([pcd0, pcd1]) print("Load their FPFH feature and evaluate.") print("Black : matching distance > 0.2") print("White : matching distance = 0") feature0 = o3d.io.read_feature(demo_data.fpfh_feature_paths[0]) feature1 = o3d.io.read_feature(demo_data.fpfh_feature_paths[1]) fpfh_tree = o3d.geometry.KDTreeFlann(feature1) for i in range(len(pcd0.points)): [_, idx, _] = fpfh_tree.search_knn_vector_xd(feature0.data[:, i], 1) dis = np.linalg.norm(pcd0.points[i] - pcd1.points[idx[0]]) c = (0.2 - np.fmin(dis, 0.2)) / 0.2 pcd0.colors[i] = [c, c, c] o3d.visualization.draw_geometries([pcd0]) print("") print("Load their L32D feature and evaluate.") print("Black : matching distance > 0.2") print("White : matching distance = 0") feature0 = o3d.io.read_feature(demo_data.l32d_feature_paths[0]) feature1 = o3d.io.read_feature(demo_data.l32d_feature_paths[1]) fpfh_tree = o3d.geometry.KDTreeFlann(feature1) for i in range(len(pcd0.points)): [_, idx, _] = fpfh_tree.search_knn_vector_xd(feature0.data[:, i], 1) dis = np.linalg.norm(pcd0.points[i] - pcd1.points[idx[0]]) c = (0.2 - np.fmin(dis, 0.2)) / 0.2 pcd0.colors[i] = [c, c, c] o3d.visualization.draw_geometries([pcd0]) print("") 

## kd_tree_search.py¶

  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 41 42 43 44 45 46 47 # ---------------------------------------------------------------------------- # - Open3D: www.open3d.org - # ---------------------------------------------------------------------------- # Copyright (c) 2018-2023 www.open3d.org # SPDX-License-Identifier: MIT # ---------------------------------------------------------------------------- """Build a KDTree and use it for neighbour search""" import open3d as o3d import numpy as np def radius_search(): print("Loading pointcloud ...") sample_pcd_data = o3d.data.PCDPointCloud() pcd = o3d.io.read_point_cloud(sample_pcd_data.path) pcd_tree = o3d.geometry.KDTreeFlann(pcd) print( "Find the neighbors of 50000th point with distance less than 0.2, and painting them green ..." ) [k, idx, _] = pcd_tree.search_radius_vector_3d(pcd.points[50000], 0.2) np.asarray(pcd.colors)[idx[1:], :] = [0, 1, 0] print("Displaying the final point cloud ...\n") o3d.visualization.draw([pcd]) def knn_search(): print("Loading pointcloud ...") sample_pcd = o3d.data.PCDPointCloud() pcd = o3d.io.read_point_cloud(sample_pcd.path) pcd_tree = o3d.geometry.KDTreeFlann(pcd) print( "Find the 2000 nearest neighbors of 50000th point, and painting them red ..." ) [k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[50000], 2000) np.asarray(pcd.colors)[idx[1:], :] = [1, 0, 0] print("Displaying the final point cloud ...\n") o3d.visualization.draw([pcd]) if __name__ == "__main__": knn_search() radius_search()