From e0a845fc452f2c1515c72a8861b62c279be43d9c Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Thu, 25 Apr 2024 13:28:06 +0200 Subject: [PATCH 1/3] fix bug, refactor code --- python/map_closures/tools/gt_closures.py | 43 +++++++++++++++--------- 1 file changed, 28 insertions(+), 15 deletions(-) diff --git a/python/map_closures/tools/gt_closures.py b/python/map_closures/tools/gt_closures.py index 8357a77..1d98b00 100644 --- a/python/map_closures/tools/gt_closures.py +++ b/python/map_closures/tools/gt_closures.py @@ -83,6 +83,7 @@ def compute_overlap(query_points: np.ndarray, ref_points: np.ndarray, voxel_size def get_gt_closures(dataset, gt_poses: List[np.ndarray], config: KISSConfig): base_dir = dataset.sequence_dir if hasattr(dataset, "sequence_dir") else "" + os.makedirs(os.path.join(base_dir, "loop_closure"), exist_ok=True) file_path_closures = os.path.join(base_dir, "loop_closure/gt_closures.txt") file_path_overlaps = os.path.join(base_dir, "loop_closure/gt_overlaps.txt") if os.path.exists(file_path_closures) and os.path.exists(file_path_overlaps): @@ -92,29 +93,41 @@ def get_gt_closures(dataset, gt_poses: List[np.ndarray], config: KISSConfig): print(f"[INFO] Found closure ground truth at {file_path_closures}") else: + print("Computing Ground Truth Closures, might take some time!") min_overlap = 0.1 sampling_distance = 2.0 + n_skip_segments = int(2 * config.data.max_range / sampling_distance) segment_indices = get_segment_indices(gt_poses, sampling_distance) - closures = [] - overlaps = [] - tbar = tqdm(segment_indices, desc="Computing Ground Truth Closures, might take some time!") - for i, query_segment in enumerate(tbar): + global_points_segments = [ + get_global_points(dataset, segment, gt_poses[segment]) for segment in segment_indices + ] + closures = np.empty((0, 2), dtype=int) + overlaps = np.empty((0,), dtype=float) + tbar = tqdm( + zip(segment_indices, global_points_segments), + total=len(segment_indices), + ) + for i, (query_segment, query_points) in enumerate(tbar): query_pose = gt_poses[query_segment[0]] - query_points = get_global_points(dataset, query_segment, gt_poses[query_segment]) - n_skip_segments = int(2 * config.data.max_range / sampling_distance) - for _, ref_segment in enumerate( - segment_indices[i + n_skip_segments :], start=n_skip_segments + 1 + for _, (ref_segment, ref_points) in enumerate( + zip( + segment_indices[i + n_skip_segments :], + global_points_segments[i + n_skip_segments :], + ), + start=n_skip_segments + 1, ): ref_pose = gt_poses[ref_segment[0]] dist = np.linalg.norm(query_pose[:3, -1] - ref_pose[:3, -1]) if dist < config.data.max_range: - ref_points = get_global_points(dataset, ref_segment, gt_poses[ref_segment]) overlap = compute_overlap(query_points, ref_points, voxel_size=0.5) if overlap > min_overlap: - for qi in query_segment: - for ri in ref_segment: - closures.append({qi, ri}) - overlaps.append(overlap) - np.savetxt(file_path_closures, np.array(closures, dtype=int)) - np.savetxt(file_path_overlaps, np.array(overlaps)) + segment_closures = np.dstack( + np.meshgrid(query_segment, ref_segment) + ).reshape(-1, 2) + closures = np.vstack((closures, segment_closures), dtype=int) + overlaps = np.hstack( + (overlaps, np.full(segment_closures.shape[0], overlap)) + ) + np.savetxt(file_path_closures, closures) + np.savetxt(file_path_overlaps, overlaps) return closures, overlaps From 82c8bd628257f7d64402c0840aff34fe9c8af3dc Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Thu, 25 Apr 2024 17:06:37 +0200 Subject: [PATCH 2/3] Add gt data link in README --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index acef2ed..feb4aa9 100644 --- a/README.md +++ b/README.md @@ -89,6 +89,8 @@ git checkout ICRA2024 ``` Our development aims to push the performances of **MapClosures** above the original results of the paper. +**Note**: You can download the ground-truth loop closure candidates for the datasets used in the paper from [here](https://www.ipb.uni-bonn.de/html/projects/gupta2024icra/MapClosuresGroundtruth.zip). When run with `-e` flag, our pipeline will search for groundtruth data under the folder at path `/loop_closure/`. If not found, it will first generate the groundtruth closures which might consume some time. + ## Acknowledgement This repository is heavily inspired by, and also depends on [KISS-ICP](https://github.com/PRBonn/kiss-icp) From 674c7f215b3046ae27c852d2353ff5476c9e9b1c Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Fri, 26 Apr 2024 10:50:54 +0200 Subject: [PATCH 3/3] Cosmetics --- python/map_closures/tools/gt_closures.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/map_closures/tools/gt_closures.py b/python/map_closures/tools/gt_closures.py index 1d98b00..e331ad8 100644 --- a/python/map_closures/tools/gt_closures.py +++ b/python/map_closures/tools/gt_closures.py @@ -84,8 +84,8 @@ def compute_overlap(query_points: np.ndarray, ref_points: np.ndarray, voxel_size def get_gt_closures(dataset, gt_poses: List[np.ndarray], config: KISSConfig): base_dir = dataset.sequence_dir if hasattr(dataset, "sequence_dir") else "" os.makedirs(os.path.join(base_dir, "loop_closure"), exist_ok=True) - file_path_closures = os.path.join(base_dir, "loop_closure/gt_closures.txt") - file_path_overlaps = os.path.join(base_dir, "loop_closure/gt_overlaps.txt") + file_path_closures = os.path.join(base_dir, "loop_closure", "gt_closures.txt") + file_path_overlaps = os.path.join(base_dir, "loop_closure", "gt_overlaps.txt") if os.path.exists(file_path_closures) and os.path.exists(file_path_overlaps): closures = np.loadtxt(file_path_closures, dtype=int) overlaps = np.loadtxt(file_path_overlaps)