mirrors/FAST_LIO_LOCALIZATION
1
0
Fork 0
mirror of https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git synced 2023-05-28 12:51:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor code

Browse Source
This commit is contained in:
HViktorTsoi
2021-08-06 15:31:20 +08:00
parent 0d7de54ce2
commit 8fbffa2802
1 changed files with 2 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
scripts/global_localization.py
View File

@@ -92,9 +92,7 @@ def publish_point_cloud(publisher, header, pc):
publisher.publish(msg) publisher.publish(msg)
def crop_global_map_in_FOV(pose_estimation): def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom):
global global_map, cur_odom
# 当前scan原点的位姿 # 当前scan原点的位姿
T_odom_to_base_link = pose_to_mat(cur_odom) T_odom_to_base_link = pose_to_mat(cur_odom)
T_map_to_base_link = np.matmul(pose_estimation, T_odom_to_base_link) T_map_to_base_link = np.matmul(pose_estimation, T_odom_to_base_link)
@@ -142,7 +140,7 @@ def global_localization(pose_estimation):
tic = time.time() tic = time.time()
global_map_in_FOV = crop_global_map_in_FOV(pose_estimation) global_map_in_FOV = crop_global_map_in_FOV(global_map, pose_estimation, cur_odom)
# 粗配准 # 粗配准
transformation, _ = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=5) transformation, _ = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=5)