From c08d95d14e815925345e84ec1bbdd47e210f1543 Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Wed, 11 Aug 2021 19:59:26 +0800 Subject: [PATCH] add open3d 0.7 support --- README.md | 2 ++ scripts/global_localization.py | 13 +++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d16e238..44ffcb1 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,8 @@ A simple localization framework that can re-localize in built maps based on [FAS ## News +- **2021-08-11:** Add **Open3D 0.7** support. + - **2021-08-09:** Migrate to **Open3D** for better performance. ## 1. Features diff --git a/scripts/global_localization.py b/scripts/global_localization.py index 254b561..79d9a4c 100755 --- a/scripts/global_localization.py +++ b/scripts/global_localization.py @@ -41,7 +41,7 @@ def msg_to_array(pc_msg): def registration_at_scale(pc_scan, pc_map, initial, scale): result_icp = o3d.registration.registration_icp( - pc_scan.voxel_down_sample(SCAN_VOXEL_SIZE * scale), pc_map.voxel_down_sample(MAP_VOXEL_SIZE * scale), + voxel_down_sample(pc_scan, SCAN_VOXEL_SIZE * scale), voxel_down_sample(pc_map, MAP_VOXEL_SIZE * scale), 1.0 * scale, initial, o3d.registration.TransformationEstimationPointToPoint(), o3d.registration.ICPConvergenceCriteria(max_iteration=20) @@ -157,12 +157,21 @@ def global_localization(pose_estimation): return False +def voxel_down_sample(pcd, voxel_size): + try: + pcd_down = pcd.voxel_down_sample(voxel_size) + except: + # for opend3d 0.7 or lower + pcd_down = o3d.geometry.voxel_down_sample(pcd, voxel_size) + return pcd_down + + def initialize_global_map(pc_msg): global global_map global_map = o3d.geometry.PointCloud() global_map.points = o3d.utility.Vector3dVector(msg_to_array(pc_msg)[:, :3]) - global_map = global_map.voxel_down_sample(MAP_VOXEL_SIZE) + global_map = voxel_down_sample(global_map, MAP_VOXEL_SIZE) rospy.loginfo('Global map received.')