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

small fixes

release/4.3a0
Varun Agrawal 2025-02-17 18:25:23 -05:00
parent 9f1aa6ae6e
commit ebfb94c4fd
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,6 +48,7 @@ def parse_arguments():
default=10, default=10,
help="The maximum number of hypotheses to keep at any time.") help="The maximum number of hypotheses to keep at any time.")
parser.add_argument( parser.add_argument(
"--plot_hypotheses",
"-p", "-p",
action="store_true", action="store_true",
help="Plot all hypotheses. NOTE: This is exponential, use with caution." help="Plot all hypotheses. NOTE: This is exponential, use with caution."
@ -173,8 +174,8 @@ class Experiment:
self.new_factors_ = HybridNonlinearFactorGraph() self.new_factors_ = HybridNonlinearFactorGraph()
self.all_factors_ = HybridNonlinearFactorGraph() self.all_factors_ = HybridNonlinearFactorGraph()
self.initial_ = Values() self.initial_ = Values()
self.plot_hypotheses: = plot_hypotheses self.plot_hypotheses = plot_hypotheses
def hybrid_loop_closure_factor(self, loop_counter, key_s, key_t, def hybrid_loop_closure_factor(self, loop_counter, key_s, key_t,
measurement: Pose2): measurement: Pose2):
@ -358,6 +359,10 @@ class Experiment:
delimiter=" ") delimiter=" ")
# Get all possible assignments # Get all possible assignments
if discrete_keys.size() > 5:
print("Too many discrete keys to plot all hypotheses. Exiting.")
exit(0)
all_assignments = gtsam.cartesianProduct(discrete_keys) all_assignments = gtsam.cartesianProduct(discrete_keys)
for idx, assignment in enumerate(all_assignments): for idx, assignment in enumerate(all_assignments):