From a560f373fa7ef0a75777af1d769906b515f54697 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Feb 2025 17:56:20 -0500
Subject: [PATCH 01/20] rename to reinitialize and separate method for saving
 results

---
 python/gtsam/examples/HybridCity10000.py | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index 5a9ef9585..99978de89 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -147,7 +147,7 @@ class Experiment:
         after_update = time.time()
         return after_update - before_update
 
-    def reInitialize(self) -> float:
+    def reinitialize(self) -> float:
         """Re-linearize, solve ALL, and re-initialize smoother."""
         print(f"================= Re-Initialize: {self.all_factors_.size()}")
         before_update = time.time()
@@ -157,7 +157,7 @@ class Experiment:
         bayesNet = linearized.eliminateSequential()
         delta: HybridValues = bayesNet.optimize()
         self.initial_ = self.initial_.retract(delta.continuous())
-        self.smoother_.reInitialize(bayesNet)
+        self.smoother_.reinitialize(bayesNet)
         after_update = time.time()
         print(f"Took {after_update - before_update} seconds.")
         return after_update - before_update
@@ -240,7 +240,7 @@ class Experiment:
                 update_count += 1
 
                 if update_count % self.relinearization_frequency == 0:
-                    self.reInitialize()
+                    self.reinitialize()
 
             #  Record timing for odometry edges only
             if key_s == key_t - 1:
@@ -271,8 +271,12 @@ class Experiment:
         total_time = end_time - start_time
         print(f"Total time: {total_time} seconds")
 
+        self.save_results(result, key_t + 1, time_list)
+
+    def save_results(self, result, final_key, time_list):
+        """Save results to file."""
         # Write results to file
-        self.write_result(result, key_t + 1, "Hybrid_City10000.txt")
+        self.write_result(result, final_key, "Hybrid_City10000.txt")
 
         # Write timing info to file
         self.write_timing_info(time_list=time_list)

From ed409c3dc20858516010f13e812a9bf6127c51c2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Feb 2025 17:56:31 -0500
Subject: [PATCH 02/20] wrap choose method

---
 gtsam/hybrid/hybrid.i | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 1e6f7bf25..20bff4971 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -152,6 +152,8 @@ class HybridBayesNet {
   gtsam::HybridGaussianFactorGraph toFactorGraph(
       const gtsam::VectorValues& measurements) const;
 
+  gtsam::GaussianBayesNet choose(const gtsam::DiscreteValues& assignment) const;
+
   gtsam::HybridValues optimize() const;
   gtsam::HybridValues sample(const gtsam::HybridValues& given) const;
   gtsam::HybridValues sample() const;

From 435e721f5ceefa5d18dc7676a7a23d5b4e7c4ff5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Feb 2025 20:00:48 -0500
Subject: [PATCH 03/20] plot all hypotheses

---
 python/gtsam/examples/HybridCity10000.py | 89 ++++++++++++++++++++++--
 1 file changed, 85 insertions(+), 4 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index 99978de89..c852e65b2 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -15,6 +15,7 @@ import time
 
 import numpy as np
 from gtsam.symbol_shorthand import L, M, X
+from matplotlib import pyplot as plt
 
 import gtsam
 from gtsam import (BetweenFactorPose2, HybridNonlinearFactor,
@@ -87,13 +88,52 @@ class City10000Dataset:
             return None, None
 
 
+def plot_estimates(gt,
+                   estimates,
+                   fignum: int,
+                   estimate_color=(0.1, 0.1, 0.9, 0.4),
+                   estimate_label="Hybrid Factor Graphs",
+                   text="graph"):
+    """Plot the City10000 estimates against the ground truth.
+
+    Args:
+        estimates (np.ndarray): The estimates trajectory as xy values.
+        fignum (int): The figure number for multiple plots.
+        estimate_color (tuple, optional): The color to use for the graph of estimates.
+            Defaults to (0.1, 0.1, 0.9, 0.4).
+        estimate_label (str, optional): Label for the estimates, used in the legend.
+            Defaults to "Hybrid Factor Graphs".
+    """
+    fig = plt.figure(fignum)
+    ax = fig.gca()
+    ax.axis('equal')
+    ax.axis((-65.0, 65.0, -75.0, 60.0))
+    ax.plot(gt[:, 0],
+            gt[:, 1],
+            '--',
+            linewidth=1,
+            color=(0.1, 0.7, 0.1, 0.5),
+            label="Ground Truth")
+    ax.plot(estimates[:, 0],
+            estimates[:, 1],
+            '-',
+            linewidth=1,
+            color=estimate_color,
+            label=estimate_label)
+    ax.legend()
+    fig.text(0.3, 0.03, text)
+
+    filename = f"city10000_{text.replace('_', ' ')}.svg"
+    fig.savefig(filename, format="svg")
+
+
 class Experiment:
     """Experiment Class"""
 
     def __init__(self,
                  filename: str,
-                 marginal_threshold: float = 0.9999,
-                 max_loop_count: int = 8000,
+                 marginal_threshold: float = 1.9999,
+                 max_loop_count: int = 100,
                  update_frequency: int = 3,
                  max_num_hypotheses: int = 10,
                  relinearization_frequency: int = 10):
@@ -200,7 +240,7 @@ class Experiment:
             num_measurements = len(pose_array)
 
             # Take the first one as the initial estimate
-            odom_pose = pose_array[0]
+            odom_pose = pose_array[np.random.choice(num_measurements)]
             if key_s == key_t - 1:
                 # Odometry factor
                 if num_measurements > 1:
@@ -271,7 +311,48 @@ class Experiment:
         total_time = end_time - start_time
         print(f"Total time: {total_time} seconds")
 
-        self.save_results(result, key_t + 1, time_list)
+        # self.save_results(result, key_t + 1, time_list)
+
+        # Get all the discrete values
+        discrete_keys = gtsam.DiscreteKeys()
+        for key in delta.discrete().keys():
+            # TODO Get cardinality from DiscreteFactor
+            discrete_keys.push_back((key, 2))
+        print("plotting all hypotheses")
+        self.plot_all_hypotheses(discrete_keys, key_t + 1)
+
+    def plot_all_hypotheses(self, discrete_keys, num_poses):
+        """Plot all possible hypotheses."""
+
+        # Get ground truth
+        gt = np.loadtxt(gtsam.findExampleDataFile("ISAM2_GT_city10000.txt"),
+                        delimiter=" ")
+
+        # print(discrete_keys)
+        # Get all possible assignments
+        all_assignments = gtsam.cartesianProduct(discrete_keys)
+
+        for idx, assignment in enumerate(all_assignments):
+            print(idx)
+            result = gtsam.Values()
+            delta = self.smoother_.hybridBayesNet().optimize(assignment)
+            result.insert_or_assign(self.initial_.retract(delta))
+
+            poses = []
+            for i in range(num_poses):
+                pose = result.atPose2(X(i))
+                poses.append((pose.x(), pose.y(), pose.theta()))
+            poses = np.asarray(poses)
+
+            assignment_string = "_".join([
+                f"{gtsam.DefaultKeyFormatter(k)}={v}"
+                for k, v in assignment.items()
+            ])
+
+            plot_estimates(gt,
+                           estimates=poses,
+                           fignum=idx,
+                           text=assignment_string)
 
     def save_results(self, result, final_key, time_list):
         """Save results to file."""

From fefc6c314bf34f354f0eefd8e7a7efa70433ec89 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 14 Feb 2025 10:42:03 -0500
Subject: [PATCH 04/20] improve plot saving

---
 python/gtsam/examples/HybridCity10000.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index c852e65b2..e429d9611 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -121,9 +121,9 @@ def plot_estimates(gt,
             color=estimate_color,
             label=estimate_label)
     ax.legend()
-    fig.text(0.3, 0.03, text)
+    fig.text(0.1, 0.03, text)
 
-    filename = f"city10000_{text.replace('_', ' ')}.svg"
+    filename = f"city10000_{text.replace(' ', '_')}.svg"
     fig.savefig(filename, format="svg")
 
 
@@ -133,7 +133,7 @@ class Experiment:
     def __init__(self,
                  filename: str,
                  marginal_threshold: float = 1.9999,
-                 max_loop_count: int = 100,
+                 max_loop_count: int = 150,
                  update_frequency: int = 3,
                  max_num_hypotheses: int = 10,
                  relinearization_frequency: int = 10):
@@ -333,7 +333,6 @@ class Experiment:
         all_assignments = gtsam.cartesianProduct(discrete_keys)
 
         for idx, assignment in enumerate(all_assignments):
-            print(idx)
             result = gtsam.Values()
             delta = self.smoother_.hybridBayesNet().optimize(assignment)
             result.insert_or_assign(self.initial_.retract(delta))
@@ -344,7 +343,7 @@ class Experiment:
                 poses.append((pose.x(), pose.y(), pose.theta()))
             poses = np.asarray(poses)
 
-            assignment_string = "_".join([
+            assignment_string = " ".join([
                 f"{gtsam.DefaultKeyFormatter(k)}={v}"
                 for k, v in assignment.items()
             ])

From 2b8df456983f9205587fc2da619cf6692d9c853c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 14 Feb 2025 10:42:24 -0500
Subject: [PATCH 05/20] check for invalid Bayes nets

---
 python/gtsam/examples/HybridCity10000.py | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index e429d9611..bc820b35b 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -334,6 +334,17 @@ class Experiment:
 
         for idx, assignment in enumerate(all_assignments):
             result = gtsam.Values()
+            gbn = self.smoother_.hybridBayesNet().choose(assignment)
+
+            # Check to see if the GBN has any nullptrs, if it does it is null overall
+            is_invalid_gbn = False
+            for i in range(gbn.size()):
+                if gbn.at(i) is None:
+                    is_invalid_gbn = True
+                    break
+            if is_invalid_gbn:
+                continue
+
             delta = self.smoother_.hybridBayesNet().optimize(assignment)
             result.insert_or_assign(self.initial_.retract(delta))
 

From 782a45484ea78254d67c0a5d5bf2722ce479e5ad Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 14 Feb 2025 10:42:55 -0500
Subject: [PATCH 06/20] more command line arguments

---
 python/gtsam/examples/HybridCity10000.py | 23 ++++++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index bc820b35b..81fb90382 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -29,6 +29,24 @@ def parse_arguments():
     parser.add_argument("--data_file",
                         help="The path to the City10000 data file",
                         default="T1_city10000_04.txt")
+    parser.add_argument(
+        "--max_loop_count",
+        "-l",
+        type=int,
+        default=10000,
+        help="The maximum number of loops to run over the dataset")
+    parser.add_argument(
+        "--update_frequency",
+        "-u",
+        type=int,
+        default=3,
+        help="After how many steps to run the smoother update.")
+    parser.add_argument(
+        "--max_num_hypotheses",
+        "-m",
+        type=int,
+        default=10,
+        help="The maximum number of hypotheses to keep at any time.")
     return parser.parse_args()
 
 
@@ -407,7 +425,10 @@ def main():
     """Main runner"""
     args = parse_arguments()
 
-    experiment = Experiment(gtsam.findExampleDataFile(args.data_file))
+    experiment = Experiment(gtsam.findExampleDataFile(args.data_file),
+                            max_loop_count=args.max_loop_count,
+                            update_frequency=args.update_frequency,
+                            max_num_hypotheses=args.max_num_hypotheses)
     experiment.run()
 
 

From d6b3c4d1d2f9dbd6e48acdfbc8e209218ac28fd7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Feb 2025 14:46:30 -0500
Subject: [PATCH 07/20] wrap more HybridBayesNet methods

---
 gtsam/hybrid/hybrid.i | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 20bff4971..5e93c85a9 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -155,6 +155,8 @@ class HybridBayesNet {
   gtsam::GaussianBayesNet choose(const gtsam::DiscreteValues& assignment) const;
 
   gtsam::HybridValues optimize() const;
+  gtsam::VectorValues optimize(const gtsam::DiscreteValues& assignment) const;
+
   gtsam::HybridValues sample(const gtsam::HybridValues& given) const;
   gtsam::HybridValues sample() const;
 

From 9f1aa6ae6e12363a891da6579bf44c0d49dab969 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Feb 2025 18:04:45 -0500
Subject: [PATCH 08/20] arg to plot hypotheses

---
 python/gtsam/examples/HybridCity10000.py | 37 +++++++++++++++---------
 1 file changed, 24 insertions(+), 13 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index 81fb90382..00807352c 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -47,6 +47,11 @@ def parse_arguments():
         type=int,
         default=10,
         help="The maximum number of hypotheses to keep at any time.")
+    parser.add_argument(
+        "-p",
+        action="store_true",
+        help="Plot all hypotheses. NOTE: This is exponential, use with caution."
+    )
     return parser.parse_args()
 
 
@@ -125,7 +130,9 @@ def plot_estimates(gt,
     fig = plt.figure(fignum)
     ax = fig.gca()
     ax.axis('equal')
-    ax.axis((-65.0, 65.0, -75.0, 60.0))
+    ax.axis((-75.0, 75.0, -75.0, 75.0))
+
+    gt = gt[:estimates.shape[0]]
     ax.plot(gt[:, 0],
             gt[:, 1],
             '--',
@@ -150,11 +157,12 @@ class Experiment:
 
     def __init__(self,
                  filename: str,
-                 marginal_threshold: float = 1.9999,
+                 marginal_threshold: float = 0.9999,
                  max_loop_count: int = 150,
                  update_frequency: int = 3,
                  max_num_hypotheses: int = 10,
-                 relinearization_frequency: int = 10):
+                 relinearization_frequency: int = 10,
+                 plot_hypotheses: bool = False):
         self.dataset_ = City10000Dataset(filename)
         self.max_loop_count = max_loop_count
         self.update_frequency = update_frequency
@@ -165,6 +173,8 @@ class Experiment:
         self.new_factors_ = HybridNonlinearFactorGraph()
         self.all_factors_ = HybridNonlinearFactorGraph()
         self.initial_ = Values()
+        
+        self.plot_hypotheses: = plot_hypotheses
 
     def hybrid_loop_closure_factor(self, loop_counter, key_s, key_t,
                                    measurement: Pose2):
@@ -215,7 +225,7 @@ class Experiment:
         bayesNet = linearized.eliminateSequential()
         delta: HybridValues = bayesNet.optimize()
         self.initial_ = self.initial_.retract(delta.continuous())
-        self.smoother_.reinitialize(bayesNet)
+        self.smoother_.reInitialize(bayesNet)
         after_update = time.time()
         print(f"Took {after_update - before_update} seconds.")
         return after_update - before_update
@@ -331,13 +341,14 @@ class Experiment:
 
         # self.save_results(result, key_t + 1, time_list)
 
-        # Get all the discrete values
-        discrete_keys = gtsam.DiscreteKeys()
-        for key in delta.discrete().keys():
-            # TODO Get cardinality from DiscreteFactor
-            discrete_keys.push_back((key, 2))
-        print("plotting all hypotheses")
-        self.plot_all_hypotheses(discrete_keys, key_t + 1)
+        if self.plot_hypotheses:
+            # Get all the discrete values
+            discrete_keys = gtsam.DiscreteKeys()
+            for key in delta.discrete().keys():
+                # TODO Get cardinality from DiscreteFactor
+                discrete_keys.push_back((key, 2))
+            print("plotting all hypotheses")
+            self.plot_all_hypotheses(discrete_keys, key_t + 1)
 
     def plot_all_hypotheses(self, discrete_keys, num_poses):
         """Plot all possible hypotheses."""
@@ -346,7 +357,6 @@ class Experiment:
         gt = np.loadtxt(gtsam.findExampleDataFile("ISAM2_GT_city10000.txt"),
                         delimiter=" ")
 
-        # print(discrete_keys)
         # Get all possible assignments
         all_assignments = gtsam.cartesianProduct(discrete_keys)
 
@@ -428,7 +438,8 @@ def main():
     experiment = Experiment(gtsam.findExampleDataFile(args.data_file),
                             max_loop_count=args.max_loop_count,
                             update_frequency=args.update_frequency,
-                            max_num_hypotheses=args.max_num_hypotheses)
+                            max_num_hypotheses=args.max_num_hypotheses,
+                            plot_hypotheses=args.plot_hypotheses)
     experiment.run()
 
 

From ebfb94c4fd597f5a4a848d7c844b64fe84a70205 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Feb 2025 18:25:23 -0500
Subject: [PATCH 09/20] small fixes

---
 python/gtsam/examples/HybridCity10000.py | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index 00807352c..d5d86157d 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -48,6 +48,7 @@ def parse_arguments():
         default=10,
         help="The maximum number of hypotheses to keep at any time.")
     parser.add_argument(
+        "--plot_hypotheses",
         "-p",
         action="store_true",
         help="Plot all hypotheses. NOTE: This is exponential, use with caution."
@@ -173,8 +174,8 @@ class Experiment:
         self.new_factors_ = HybridNonlinearFactorGraph()
         self.all_factors_ = HybridNonlinearFactorGraph()
         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,
                                    measurement: Pose2):
@@ -358,6 +359,10 @@ class Experiment:
                         delimiter=" ")
 
         # 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)
 
         for idx, assignment in enumerate(all_assignments):

From 6919bae1b799237d06f4bfa0ff8ba93724a69883 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 18 Feb 2025 10:23:26 -0500
Subject: [PATCH 10/20] generate a single plot with subplots

---
 python/gtsam/examples/HybridCity10000.py | 85 ++++++++++++------------
 1 file changed, 42 insertions(+), 43 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index d5d86157d..2e846cca0 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -112,45 +112,44 @@ class City10000Dataset:
             return None, None
 
 
-def plot_estimates(gt,
-                   estimates,
-                   fignum: int,
-                   estimate_color=(0.1, 0.1, 0.9, 0.4),
-                   estimate_label="Hybrid Factor Graphs",
-                   text="graph"):
+def plot_all_results(ground_truth,
+                     all_results,
+                     estimate_color=(0.1, 0.1, 0.9, 0.4),
+                     estimate_label="Hybrid Factor Graphs"):
     """Plot the City10000 estimates against the ground truth.
 
     Args:
-        estimates (np.ndarray): The estimates trajectory as xy values.
-        fignum (int): The figure number for multiple plots.
+        ground_truth: The ground truth trajectory as xy values.
+        all_results (List[Tuple(np.ndarray, str)]): All the estimates trajectory as xy values, as well as assginment strings.
         estimate_color (tuple, optional): The color to use for the graph of estimates.
             Defaults to (0.1, 0.1, 0.9, 0.4).
         estimate_label (str, optional): Label for the estimates, used in the legend.
             Defaults to "Hybrid Factor Graphs".
     """
-    fig = plt.figure(fignum)
-    ax = fig.gca()
-    ax.axis('equal')
-    ax.axis((-75.0, 75.0, -75.0, 75.0))
+    print(len(all_results))
+    fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
+    for i, (estimates, text) in enumerate(all_results):
+        ax = axes[i]
+        ax.axis('equal')
+        ax.axis((-75.0, 75.0, -75.0, 75.0))
 
-    gt = gt[:estimates.shape[0]]
-    ax.plot(gt[:, 0],
-            gt[:, 1],
-            '--',
-            linewidth=1,
-            color=(0.1, 0.7, 0.1, 0.5),
-            label="Ground Truth")
-    ax.plot(estimates[:, 0],
-            estimates[:, 1],
-            '-',
-            linewidth=1,
-            color=estimate_color,
-            label=estimate_label)
-    ax.legend()
-    fig.text(0.1, 0.03, text)
+        gt = ground_truth[:estimates.shape[0]]
+        ax.plot(gt[:, 0],
+                gt[:, 1],
+                '--',
+                linewidth=1,
+                color=(0.1, 0.7, 0.1, 0.5),
+                label="Ground Truth")
+        ax.plot(estimates[:, 0],
+                estimates[:, 1],
+                '-',
+                linewidth=1,
+                color=estimate_color,
+                label=estimate_label)
+        ax.legend()
+        ax.text(0.0, 100.0, text)
 
-    filename = f"city10000_{text.replace(' ', '_')}.svg"
-    fig.savefig(filename, format="svg")
+    fig.savefig("city10000_results.svg", format="svg")
 
 
 class Experiment:
@@ -269,7 +268,8 @@ class Experiment:
             num_measurements = len(pose_array)
 
             # Take the first one as the initial estimate
-            odom_pose = pose_array[np.random.choice(num_measurements)]
+            # odom_pose = pose_array[np.random.choice(num_measurements)]
+            odom_pose = pose_array[0]
             if key_s == key_t - 1:
                 # Odometry factor
                 if num_measurements > 1:
@@ -358,14 +358,16 @@ class Experiment:
         gt = np.loadtxt(gtsam.findExampleDataFile("ISAM2_GT_city10000.txt"),
                         delimiter=" ")
 
-        # Get all possible assignments
-        if discrete_keys.size() > 5:
-            print("Too many discrete keys to plot all hypotheses. Exiting.")
-            exit(0)
+        dkeys = gtsam.DiscreteKeys()
+        for i in range(discrete_keys.size()):
+            key, cardinality = discrete_keys.at(i)
+            if key not in self.smoother_.fixedValues().keys():
+                dkeys.push_back((key, cardinality))
 
-        all_assignments = gtsam.cartesianProduct(discrete_keys)
+        all_assignments = gtsam.cartesianProduct(dkeys)
 
-        for idx, assignment in enumerate(all_assignments):
+        all_results = []
+        for assignment in all_assignments:
             result = gtsam.Values()
             gbn = self.smoother_.hybridBayesNet().choose(assignment)
 
@@ -381,21 +383,18 @@ class Experiment:
             delta = self.smoother_.hybridBayesNet().optimize(assignment)
             result.insert_or_assign(self.initial_.retract(delta))
 
-            poses = []
+            poses = np.zeros((num_poses, 3))
             for i in range(num_poses):
                 pose = result.atPose2(X(i))
-                poses.append((pose.x(), pose.y(), pose.theta()))
-            poses = np.asarray(poses)
+                poses[i] = np.asarray((pose.x(), pose.y(), pose.theta()))
 
             assignment_string = " ".join([
                 f"{gtsam.DefaultKeyFormatter(k)}={v}"
                 for k, v in assignment.items()
             ])
+            all_results.append((poses, assignment_string))
 
-            plot_estimates(gt,
-                           estimates=poses,
-                           fignum=idx,
-                           text=assignment_string)
+        plot_all_results(gt, all_results)
 
     def save_results(self, result, final_key, time_list):
         """Save results to file."""

From 82e2c4f80e3468ca1046744b46b298f04d60b7c9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 18 Feb 2025 11:44:32 -0500
Subject: [PATCH 11/20] return whether loop is ambiguous

---
 python/gtsam/examples/HybridCity10000.py | 20 ++++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index 2e846cca0..e4f9b0b7d 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -85,13 +85,16 @@ class City10000Dataset:
         """Read a `line` from the dataset, separated by the `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"""
         parts = self.read_line(line)
 
         key_s = int(parts[1])
         key_t = int(parts[3])
 
+        is_ambiguous_loop = bool(int(parts[4]))
+
         num_measurements = int(parts[5])
         pose_array = [Pose2()] * num_measurements
 
@@ -101,7 +104,7 @@ class City10000Dataset:
             rad = float(parts[8 + 3 * i])
             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):
         """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.
             Defaults to "Hybrid Factor Graphs".
     """
-    print(len(all_results))
     fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
     for i, (estimates, text) in enumerate(all_results):
         ax = axes[i]
@@ -259,7 +261,7 @@ class Experiment:
         start_time = time.time()
 
         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:
                 break
             key_s = keys[0]
@@ -293,8 +295,14 @@ class Experiment:
                     self.initial_.atPose2(X(key_s)) * odom_pose)
             else:
                 # Loop closure
-                loop_factor = self.hybrid_loop_closure_factor(
-                    loop_count, key_s, key_t, odom_pose)
+                if is_ambiguous_loop:
+                    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(f"Loop closure: {key_s} {key_t}")

From 0c349f7805af37a034927653078d7c3aea8fe3a0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 18 Feb 2025 11:45:13 -0500
Subject: [PATCH 12/20] update noise model so we don't get ILS

---
 python/gtsam/examples/HybridCity10000.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index e4f9b0b7d..d52b86c3e 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -64,7 +64,7 @@ prior_noise_model = gtsam.noiseModel.Diagonal.Sigmas(
     np.asarray([0.0001, 0.0001, 0.0001]))
 
 pose_noise_model = gtsam.noiseModel.Diagonal.Sigmas(
-    np.asarray([1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0]))
+    np.asarray([1.0 / 20.0, 1.0 / 20.0, 1.0 / 100.0]))
 pose_noise_constant = pose_noise_model.negLogConstant()
 
 

From 2062e0124e6c05e5afbf088886332f4bf3d17548 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 18 Feb 2025 15:57:36 -0500
Subject: [PATCH 13/20] add discrete assignments as labels

---
 python/gtsam/examples/HybridCity10000.py | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index d52b86c3e..92102a0e3 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -118,7 +118,8 @@ class City10000Dataset:
 def plot_all_results(ground_truth,
                      all_results,
                      estimate_color=(0.1, 0.1, 0.9, 0.4),
-                     estimate_label="Hybrid Factor Graphs"):
+                     estimate_label="Hybrid Factor Graphs",
+                     text=""):
     """Plot the City10000 estimates against the ground truth.
 
     Args:
@@ -130,7 +131,7 @@ def plot_all_results(ground_truth,
             Defaults to "Hybrid Factor Graphs".
     """
     fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
-    for i, (estimates, text) in enumerate(all_results):
+    for i, (estimates, s) in enumerate(all_results):
         ax = axes[i]
         ax.axis('equal')
         ax.axis((-75.0, 75.0, -75.0, 75.0))
@@ -149,7 +150,11 @@ def plot_all_results(ground_truth,
                 color=estimate_color,
                 label=estimate_label)
         ax.legend()
-        ax.text(0.0, 100.0, text)
+        ax.text(0.0, 100.0, s)
+
+    num_chunks = int(np.ceil(len(text) / 90))
+    text = "\n".join(text[i * 60:(i + 1) * 60] for i in range(num_chunks))
+    fig.text(0.0, 0.015, s=text)
 
     fig.savefig("city10000_results.svg", format="svg")
 
@@ -371,6 +376,9 @@ class Experiment:
             key, cardinality = discrete_keys.at(i)
             if key not in self.smoother_.fixedValues().keys():
                 dkeys.push_back((key, cardinality))
+        fixed_values_str = "_".join(
+            f"{gtsam.DefaultKeyFormatter(k)}:{v}"
+            for k, v in self.smoother_.fixedValues().items())
 
         all_assignments = gtsam.cartesianProduct(dkeys)
 
@@ -402,7 +410,7 @@ class Experiment:
             ])
             all_results.append((poses, assignment_string))
 
-        plot_all_results(gt, all_results)
+        plot_all_results(gt, all_results, text=fixed_values_str)
 
     def save_results(self, result, final_key, time_list):
         """Save results to file."""

From 518b067104995a2ea71c2ab0dfe485d5a482d5f9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 20 Feb 2025 13:01:04 -0500
Subject: [PATCH 14/20] better filenaming and improved plotting

---
 python/gtsam/examples/HybridCity10000.py | 36 +++++++++++++++++-------
 1 file changed, 26 insertions(+), 10 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index 92102a0e3..5f54701d9 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -117,24 +117,33 @@ class City10000Dataset:
 
 def plot_all_results(ground_truth,
                      all_results,
+                     iters=0,
                      estimate_color=(0.1, 0.1, 0.9, 0.4),
                      estimate_label="Hybrid Factor Graphs",
-                     text=""):
+                     text="",
+                     filename="city10000_results.svg"):
     """Plot the City10000 estimates against the ground truth.
 
     Args:
         ground_truth: The ground truth trajectory as xy values.
-        all_results (List[Tuple(np.ndarray, str)]): All the estimates trajectory as xy values, as well as assginment strings.
+        all_results (List[Tuple(np.ndarray, str)]): All the estimates trajectory as xy values,
+            as well as assginment strings.
         estimate_color (tuple, optional): The color to use for the graph of estimates.
             Defaults to (0.1, 0.1, 0.9, 0.4).
         estimate_label (str, optional): Label for the estimates, used in the legend.
             Defaults to "Hybrid Factor Graphs".
     """
-    fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
+    if len(all_results) == 1:
+        fig, axes = plt.subplots(1, 1)
+        axes = [axes]
+    else:
+        fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
+        axes = axes.flatten()
+
     for i, (estimates, s) in enumerate(all_results):
         ax = axes[i]
         ax.axis('equal')
-        ax.axis((-75.0, 75.0, -75.0, 75.0))
+        ax.axis((-75.0, 100.0, -75.0, 75.0))
 
         gt = ground_truth[:estimates.shape[0]]
         ax.plot(gt[:, 0],
@@ -149,14 +158,17 @@ def plot_all_results(ground_truth,
                 linewidth=1,
                 color=estimate_color,
                 label=estimate_label)
-        ax.legend()
-        ax.text(0.0, 100.0, s)
+        # ax.legend()
+        # Plot text `s` at (x, y) on axis
+        ax.text(-60.0, 60.0, s)
+
+    fig.suptitle(f"After {iters} iterations")
 
     num_chunks = int(np.ceil(len(text) / 90))
     text = "\n".join(text[i * 60:(i + 1) * 60] for i in range(num_chunks))
     fig.text(0.0, 0.015, s=text)
 
-    fig.savefig("city10000_results.svg", format="svg")
+    fig.savefig(filename, format="svg")
 
 
 class Experiment:
@@ -362,9 +374,9 @@ class Experiment:
                 # TODO Get cardinality from DiscreteFactor
                 discrete_keys.push_back((key, 2))
             print("plotting all hypotheses")
-            self.plot_all_hypotheses(discrete_keys, key_t + 1)
+            self.plot_all_hypotheses(discrete_keys, key_t + 1, index)
 
-    def plot_all_hypotheses(self, discrete_keys, num_poses):
+    def plot_all_hypotheses(self, discrete_keys, num_poses, num_iters=0):
         """Plot all possible hypotheses."""
 
         # Get ground truth
@@ -410,7 +422,11 @@ class Experiment:
             ])
             all_results.append((poses, assignment_string))
 
-        plot_all_results(gt, all_results, text=fixed_values_str)
+        plot_all_results(gt,
+                         all_results,
+                         iters=num_iters,
+                         text=fixed_values_str,
+                         filename=f"city10000_results_{num_iters}.svg")
 
     def save_results(self, result, final_key, time_list):
         """Save results to file."""

From 648634f42c1537ca39257510d57139f15063f689 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 1 Mar 2025 00:20:16 -0500
Subject: [PATCH 15/20] helper method for printing DiscreteValues

---
 gtsam/discrete/DiscreteValues.cpp | 5 +++++
 gtsam/discrete/DiscreteValues.h   | 5 +++++
 gtsam/discrete/discrete.i         | 5 +++++
 3 files changed, 15 insertions(+)

diff --git a/gtsam/discrete/DiscreteValues.cpp b/gtsam/discrete/DiscreteValues.cpp
index 3c3ed4468..2bcc15fc4 100644
--- a/gtsam/discrete/DiscreteValues.cpp
+++ b/gtsam/discrete/DiscreteValues.cpp
@@ -145,6 +145,11 @@ string DiscreteValues::html(const KeyFormatter& keyFormatter,
 }
 
 /* ************************************************************************ */
+void PrintDiscreteValues(const DiscreteValues& values, const std::string& s,
+                         const KeyFormatter& keyFormatter) {
+  values.print(s, keyFormatter);
+}
+
 string markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter,
                 const DiscreteValues::Names& names) {
   return values.markdown(keyFormatter, names);
diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h
index 0644e0c16..2ab152e08 100644
--- a/gtsam/discrete/DiscreteValues.h
+++ b/gtsam/discrete/DiscreteValues.h
@@ -188,6 +188,11 @@ inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
   return DiscreteValues::CartesianProduct(keys);
 }
 
+/// Free version of print for wrapper
+void PrintDiscreteValues(
+    const DiscreteValues& values, const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
 /// Free version of markdown.
 std::string GTSAM_EXPORT
 markdown(const DiscreteValues& values,
diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 5e4d8d22d..6b5d747b1 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -22,6 +22,11 @@ class DiscreteKeys {
 // DiscreteValues is added in specializations/discrete.h as a std::map
 std::vector<gtsam::DiscreteValues> cartesianProduct(
     const gtsam::DiscreteKeys& keys);
+
+void PrintDiscreteValues(
+    const gtsam::DiscreteValues& values, const std::string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+
 string markdown(
     const gtsam::DiscreteValues& values,
     const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);

From 42d7160b9a2d3a85858da37f03f0947e90a5ca5f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 1 Mar 2025 00:21:04 -0500
Subject: [PATCH 16/20] fix return type of DiscreteSearch method

---
 gtsam/discrete/discrete.i | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 6b5d747b1..74de7a922 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -477,9 +477,9 @@ class DiscreteSearchSolution {
 };
 
 class DiscreteSearch {
-  static DiscreteSearch FromFactorGraph(const gtsam::DiscreteFactorGraph& factorGraph,
-                                        const gtsam::Ordering& ordering,
-                                        bool buildJunctionTree = false);
+  static gtsam::DiscreteSearch FromFactorGraph(
+      const gtsam::DiscreteFactorGraph& factorGraph,
+      const gtsam::Ordering& ordering, bool buildJunctionTree = false);
 
   DiscreteSearch(const gtsam::DiscreteEliminationTree& etree);
   DiscreteSearch(const gtsam::DiscreteJunctionTree& junctionTree);

From 971ebfaae76cc6a1105fb2905ae284951f15af1a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 1 Mar 2025 00:22:15 -0500
Subject: [PATCH 17/20] add probability values for modes in each subplot and
 better plotting

---
 python/gtsam/examples/HybridCity10000.py | 30 ++++++++++++++++++------
 1 file changed, 23 insertions(+), 7 deletions(-)

diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index 5f54701d9..e1f4b9d05 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -112,7 +112,7 @@ class City10000Dataset:
         if line:
             return self.parse_line(line)
         else:
-            return None, None
+            return None, None, None
 
 
 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)
         axes = axes.flatten()
 
-    for i, (estimates, s) in enumerate(all_results):
+    for i, (estimates, s, prob) in enumerate(all_results):
         ax = axes[i]
         ax.axis('equal')
         ax.axis((-75.0, 100.0, -75.0, 75.0))
@@ -159,14 +159,18 @@ def plot_all_results(ground_truth,
                 color=estimate_color,
                 label=estimate_label)
         # ax.legend()
-        # Plot text `s` at (x, y) on axis
-        ax.text(-60.0, 60.0, s)
+        ax.set_title(f"P={prob:.3f}\n{s}", fontdict={'fontsize': 10})
 
     fig.suptitle(f"After {iters} iterations")
 
     num_chunks = int(np.ceil(len(text) / 90))
     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")
 
@@ -388,7 +392,7 @@ class Experiment:
             key, cardinality = discrete_keys.at(i)
             if key not in self.smoother_.fixedValues().keys():
                 dkeys.push_back((key, cardinality))
-        fixed_values_str = "_".join(
+        fixed_values_str = " ".join(
             f"{gtsam.DefaultKeyFormatter(k)}:{v}"
             for k, v in self.smoother_.fixedValues().items())
 
@@ -420,7 +424,19 @@ class Experiment:
                 f"{gtsam.DefaultKeyFormatter(k)}={v}"
                 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,
                          all_results,

From db492ed3f8ce9a2a07c9167c9bba216c1ec9927f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 1 Mar 2025 01:16:32 -0500
Subject: [PATCH 18/20] check CI

---
 gtsam/discrete/discrete.i | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index 74de7a922..cf3f98fe6 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -23,9 +23,9 @@ class DiscreteKeys {
 std::vector<gtsam::DiscreteValues> cartesianProduct(
     const gtsam::DiscreteKeys& keys);
 
-void PrintDiscreteValues(
-    const gtsam::DiscreteValues& values, const std::string& s = "",
-    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+// void PrintDiscreteValues(
+//     const gtsam::DiscreteValues& values, const std::string& s = "",
+//     const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
 
 string markdown(
     const gtsam::DiscreteValues& values,

From 02f2ef20c1d12f5a661dafa081979aad86f9d292 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 1 Mar 2025 01:19:03 -0500
Subject: [PATCH 19/20] Revert "check CI"

This reverts commit db492ed3f8ce9a2a07c9167c9bba216c1ec9927f.
---
 gtsam/discrete/discrete.i | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
index cf3f98fe6..74de7a922 100644
--- a/gtsam/discrete/discrete.i
+++ b/gtsam/discrete/discrete.i
@@ -23,9 +23,9 @@ class DiscreteKeys {
 std::vector<gtsam::DiscreteValues> cartesianProduct(
     const gtsam::DiscreteKeys& keys);
 
-// void PrintDiscreteValues(
-//     const gtsam::DiscreteValues& values, const std::string& s = "",
-//     const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+void PrintDiscreteValues(
+    const gtsam::DiscreteValues& values, const std::string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
 
 string markdown(
     const gtsam::DiscreteValues& values,

From ce6b1461e1f2ad122459ac3d5577406095aefd70 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 1 Mar 2025 01:19:38 -0500
Subject: [PATCH 20/20] set GTSAM_EXPORT for PrintDiscreteValues

---
 gtsam/discrete/DiscreteValues.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h
index 2ab152e08..46d865a0b 100644
--- a/gtsam/discrete/DiscreteValues.h
+++ b/gtsam/discrete/DiscreteValues.h
@@ -189,9 +189,9 @@ inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
 }
 
 /// Free version of print for wrapper
-void PrintDiscreteValues(
-    const DiscreteValues& values, const std::string& s = "",
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+void GTSAM_EXPORT
+PrintDiscreteValues(const DiscreteValues& values, const std::string& s = "",
+                    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
 /// Free version of markdown.
 std::string GTSAM_EXPORT