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

add probability values for modes in each subplot and better plotting

release/4.3a0
Varun Agrawal 2025-03-01 00:22:15 -05:00
parent 42d7160b9a
commit 971ebfaae7
1 changed files with 23 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -112,7 +112,7 @@ class City10000Dataset:
if line: if line:
return self.parse_line(line) return self.parse_line(line)
else: else:
return None, None return None, None, None
def plot_all_results(ground_truth, def plot_all_results(ground_truth,
@ -140,7 +140,7 @@ def plot_all_results(ground_truth,
fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2) fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
axes = axes.flatten() axes = axes.flatten()
for i, (estimates, s) in enumerate(all_results): for i, (estimates, s, prob) in enumerate(all_results):
ax = axes[i] ax = axes[i]
ax.axis('equal') ax.axis('equal')
ax.axis((-75.0, 100.0, -75.0, 75.0)) ax.axis((-75.0, 100.0, -75.0, 75.0))
@ -159,14 +159,18 @@ def plot_all_results(ground_truth,
color=estimate_color, color=estimate_color,
label=estimate_label) label=estimate_label)
# ax.legend() # ax.legend()
# Plot text `s` at (x, y) on axis ax.set_title(f"P={prob:.3f}\n{s}", fontdict={'fontsize': 10})
ax.text(-60.0, 60.0, s)
fig.suptitle(f"After {iters} iterations") fig.suptitle(f"After {iters} iterations")
num_chunks = int(np.ceil(len(text) / 90)) num_chunks = int(np.ceil(len(text) / 90))
text = "\n".join(text[i * 60:(i + 1) * 60] for i in range(num_chunks)) text = "\n".join(text[i * 60:(i + 1) * 60] for i in range(num_chunks))
fig.text(0.0, 0.015, s=text) fig.text(0.5,
0.015,
s=text,
wrap=True,
horizontalalignment='center',
fontsize=12)
fig.savefig(filename, format="svg") fig.savefig(filename, format="svg")
@ -388,7 +392,7 @@ class Experiment:
key, cardinality = discrete_keys.at(i) key, cardinality = discrete_keys.at(i)
if key not in self.smoother_.fixedValues().keys(): if key not in self.smoother_.fixedValues().keys():
dkeys.push_back((key, cardinality)) dkeys.push_back((key, cardinality))
fixed_values_str = "_".join( fixed_values_str = " ".join(
f"{gtsam.DefaultKeyFormatter(k)}:{v}" f"{gtsam.DefaultKeyFormatter(k)}:{v}"
for k, v in self.smoother_.fixedValues().items()) for k, v in self.smoother_.fixedValues().items())
@ -420,7 +424,19 @@ class Experiment:
f"{gtsam.DefaultKeyFormatter(k)}={v}" f"{gtsam.DefaultKeyFormatter(k)}={v}"
for k, v in assignment.items() for k, v in assignment.items()
]) ])
all_results.append((poses, assignment_string))
conditional = self.smoother_.hybridBayesNet().at(
self.smoother_.hybridBayesNet().size() - 1).asDiscrete()
discrete_values = self.smoother_.fixedValues()
for k, v in assignment.items():
discrete_values[k] = v
if conditional is None:
probability = 1.0
else:
probability = conditional.evaluate(discrete_values)
all_results.append((poses, assignment_string, probability))
plot_all_results(gt, plot_all_results(gt,
all_results, all_results,