From 339268dc9314fb15fedd2b7c5f8f61ffc7463291 Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Mon, 9 Aug 2021 23:34:49 +0800 Subject: [PATCH] Migrate to Open3D* for better performance. --- README.md | 17 ++++++++-- rviz_cfg/localization.rviz | 25 +++++++------- scripts/global_localization.py | 61 ++++++++++++---------------------- 3 files changed, 49 insertions(+), 54 deletions(-) diff --git a/README.md b/README.md index 622bf4d..1d77063 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,10 @@ A simple localization framework that can re-localize in built maps based on [FAST-LIO](https://github.com/hku-mars/FAST_LIO). +## News + +- Migrate to **Open3D** for better performance. + ## 1. Features - Realtime 3D global localization in a pre-built point cloud map. By fusing low-frequency global localization (about 0.5~0.2Hz), and high-frequency odometry from FAST-LIO, the entire system is computationally efficient. @@ -35,10 +39,17 @@ This part of dependency is consistent with FAST-LIO, please refer to the documen - [ros_numpy](https://github.com/eric-wieser/ros_numpy) -- [python-pcl](https://github.com/strawlab/python-pcl) +- [Open3d](https://github.com/strawlab/python-pcl) + +Notice that, there may be issue when installing **Open3D** directly using pip in **Python2.7**, you may firstly install **pyrsistent**: +```shell +pip install pyrsistent==0.15 +``` +Then +``` +pip install open3d +``` -Notice that, if using **Ubuntu 18.04** with native **PCL 1.8**, there may be issue after installing **python-pcl** through pip, - please refer to https://github.com/barrygxwan/Python-PCL-Ubuntu18.04, download the **.whl** file and then install. ## 3. Build Clone the repository and catkin_make: diff --git a/rviz_cfg/localization.rviz b/rviz_cfg/localization.rviz index e13f15a..ee3b6cf 100644 --- a/rviz_cfg/localization.rviz +++ b/rviz_cfg/localization.rviz @@ -5,6 +5,7 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /Grid1 - /mapping1/surround1 - /mapping1/currPoints1 - /mapping1/currPoints1/Autocompute Value Bounds1 @@ -47,10 +48,10 @@ Visualization Manager: Class: "" Displays: - Alpha: 1 - Cell Size: 1000 + Cell Size: 0.20000000298023224 Class: rviz/Grid Color: 160; 160; 164 - Enabled: false + Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines @@ -61,9 +62,9 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 40 - Reference Frame: - Value: false + Plane Cell Count: 10 + Reference Frame: body + Value: true - Class: rviz/Axes Enabled: false Length: 0.699999988079071 @@ -265,7 +266,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Alpha: 0.30000001192092896 + - Alpha: 0.6000000238418579 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -397,25 +398,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 109.72235107421875 + Distance: 52.648162841796875 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 17.5119571685791 + Y: -13.623329162597656 + Z: 4.375192642211914 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.0497967004776 + Pitch: 1.5697963237762451 Target Frame: body Value: Orbit (rviz) - Yaw: 1.9003976583480835 + Yaw: 1.278587818145752 Saved: ~ Window Geometry: Displays: diff --git a/scripts/global_localization.py b/scripts/global_localization.py index fb12207..254b561 100755 --- a/scripts/global_localization.py +++ b/scripts/global_localization.py @@ -6,7 +6,7 @@ import copy import thread import time -import pcl +import open3d as o3d import rospy import ros_numpy from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion @@ -40,30 +40,14 @@ def msg_to_array(pc_msg): def registration_at_scale(pc_scan, pc_map, initial, scale): - try: - sor = pc_scan.make_voxel_grid_filter() - sor.set_leaf_size(SCAN_VOXEL_SIZE * scale, SCAN_VOXEL_SIZE * scale, SCAN_VOXEL_SIZE * 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), + 1.0 * scale, initial, + o3d.registration.TransformationEstimationPointToPoint(), + o3d.registration.ICPConvergenceCriteria(max_iteration=20) + ) - # 用初始解转换到对应坐标系 - pc = np.array(sor.filter()) - pc = np.column_stack([pc, np.ones(pc.shape[0]).reshape(-1, 1)]) - pc_in_map = (np.matmul(initial, pc.T)).T - scan_tobe_mapped = pcl.PointCloud() - scan_tobe_mapped.from_array(pc_in_map[:, :3].astype(np.float32)) - - # 对地图降采样 - sor = pc_map.make_voxel_grid_filter() - sor.set_leaf_size(MAP_VOXEL_SIZE * scale, MAP_VOXEL_SIZE * scale, MAP_VOXEL_SIZE * scale) - map_down = sor.filter() - - icp = map_down.make_IterativeClosestPoint() - converged, transformation, estimate, fitness = \ - icp.icp(scan_tobe_mapped, map_down, max_iter=10) - # 这里要将初始解进行变换， 因为icp估计的是精确位置到初始解的delta - return np.matmul(transformation, initial), fitness - except Exception as e: - rospy.logerr('{}'.format(e)) - return initial, 1e9 + return result_icp.transformation, result_icp.fitness def inverse_se3(trans): @@ -99,7 +83,7 @@ def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom): T_base_link_to_map = inverse_se3(T_map_to_base_link) # 把地图转换到lidar系下 - global_map_in_map = np.array(global_map) + global_map_in_map = np.array(global_map.points) global_map_in_map = np.column_stack([global_map_in_map, np.ones(len(global_map_in_map))]) global_map_in_base_link = np.matmul(T_base_link_to_map, global_map_in_map.T).T @@ -118,13 +102,13 @@ def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom): (global_map_in_base_link[:, 0] < FOV_FAR) & (np.abs(np.arctan2(global_map_in_base_link[:, 1], global_map_in_base_link[:, 0])) < FOV / 2.0) ) - global_map_in_FOV = pcl.PointCloud() - global_map_in_FOV.from_array(np.squeeze(global_map_in_map[indices, :3]).astype(np.float32)) + global_map_in_FOV = o3d.geometry.PointCloud() + global_map_in_FOV.points = o3d.utility.Vector3dVector(np.squeeze(global_map_in_map[indices, :3])) # 发布fov内点云 header = cur_odom.header header.frame_id = 'map' - publish_point_cloud(pub_submap, header, global_map_in_FOV.to_array()[::10]) + publish_point_cloud(pub_submap, header, np.array(global_map_in_FOV.points)[::10]) return global_map_in_FOV @@ -153,7 +137,7 @@ def global_localization(pose_estimation): rospy.loginfo('') # 当全局定位成功时才更新map2odom - if fitness < LOCALIZATION_TH: + if fitness > LOCALIZATION_TH: # T_map_to_odom = np.matmul(transformation, pose_estimation) T_map_to_odom = transformation @@ -175,11 +159,10 @@ def global_localization(pose_estimation): def initialize_global_map(pc_msg): global global_map - global_map = pcl.PointCloud() - global_map.from_array(msg_to_array(pc_msg).astype(np.float32)) - sor = global_map.make_voxel_grid_filter() - sor.set_leaf_size(MAP_VOXEL_SIZE, MAP_VOXEL_SIZE, MAP_VOXEL_SIZE) - global_map = sor.filter() + + 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) rospy.loginfo('Global map received.') @@ -202,8 +185,8 @@ def cb_save_cur_scan(pc_msg): pc_msg.fields[3], pc_msg.fields[7]] pc = msg_to_array(pc_msg) - cur_scan = pcl.PointCloud() - cur_scan.from_array(pc.astype(np.float32)) + cur_scan = o3d.geometry.PointCloud() + cur_scan.points = o3d.utility.Vector3dVector(pc[:, :3]) def thread_localization(): @@ -223,14 +206,14 @@ if __name__ == '__main__': FREQ_LOCALIZATION = 0.5 # The threshold of global localization, - # only those scan2map-matching with lower fitness than LOCALIZATION_TH will be taken - LOCALIZATION_TH = 0.2 + # only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken + LOCALIZATION_TH = 0.95 # FOV(rad), modify this according to your LiDAR type FOV = 1.6 # The farthest distance(meters) within FOV - FOV_FAR = 300 + FOV_FAR = 150 rospy.init_node('fast_lio_localization') rospy.loginfo('Localization Node Inited...')