12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

return whether loop is ambiguous

release/4.3a0
Varun Agrawal 2025-02-18 11:44:32 -05:00
parent 6919bae1b7
commit 82e2c4f80e
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
python/gtsam/examples/HybridCity10000.py
View File

@ -85,13 +85,16 @@ class City10000Dataset:
"""Read a `line` from the dataset, separated by the `delimiter`.""" """Read a `line` from the dataset, separated by the `delimiter`."""
return line.split(delimiter) return line.split(delimiter)
def parse_line(self, line: str) -> tuple[list[Pose2], tuple[int, int]]: def parse_line(self,
line: str) -> tuple[list[Pose2], tuple[int, int], bool]:
"""Parse line from file""" """Parse line from file"""
parts = self.read_line(line) parts = self.read_line(line)
key_s = int(parts[1]) key_s = int(parts[1])
key_t = int(parts[3]) key_t = int(parts[3])
is_ambiguous_loop = bool(int(parts[4]))
num_measurements = int(parts[5]) num_measurements = int(parts[5])
pose_array = [Pose2()] * num_measurements pose_array = [Pose2()] * num_measurements
@ -101,7 +104,7 @@ class City10000Dataset:
rad = float(parts[8 + 3 * i]) rad = float(parts[8 + 3 * i])
pose_array[i] = Pose2(x, y, rad) pose_array[i] = Pose2(x, y, rad)
return pose_array, (key_s, key_t) return pose_array, (key_s, key_t), is_ambiguous_loop
def next(self): def next(self):
"""Read and parse the next line.""" """Read and parse the next line."""
@ -126,7 +129,6 @@ def plot_all_results(ground_truth,
estimate_label (str, optional): Label for the estimates, used in the legend. estimate_label (str, optional): Label for the estimates, used in the legend.
Defaults to "Hybrid Factor Graphs". Defaults to "Hybrid Factor Graphs".
""" """
print(len(all_results))
fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2) fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
for i, (estimates, text) in enumerate(all_results): for i, (estimates, text) in enumerate(all_results):
ax = axes[i] ax = axes[i]
@ -259,7 +261,7 @@ class Experiment:
start_time = time.time() start_time = time.time()
while index < self.max_loop_count: while index < self.max_loop_count:
pose_array, keys = self.dataset_.next() pose_array, keys, is_ambiguous_loop = self.dataset_.next()
if pose_array is None: if pose_array is None:
break break
key_s = keys[0] key_s = keys[0]
@ -293,8 +295,14 @@ class Experiment:
self.initial_.atPose2(X(key_s)) * odom_pose) self.initial_.atPose2(X(key_s)) * odom_pose)
else: else:
# Loop closure # Loop closure
loop_factor = self.hybrid_loop_closure_factor( if is_ambiguous_loop:
loop_count, key_s, key_t, odom_pose) loop_factor = self.hybrid_loop_closure_factor(
loop_count, key_s, key_t, odom_pose)
else:
loop_factor = BetweenFactorPose2(X(key_s), X(key_t),
odom_pose,
pose_noise_model)
# print loop closure event keys: # print loop closure event keys:
print(f"Loop closure: {key_s} {key_t}") print(f"Loop closure: {key_s} {key_t}")