Merge pull request #2027 from borglab/city10000-py

Improvements to HybridCity10000 python script

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);

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 GTSAM_EXPORT
+PrintDiscreteValues(const DiscreteValues& values, const std::string& s = "",
+                    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
 /// Free version of markdown.
 std::string GTSAM_EXPORT
 markdown(const DiscreteValues& values,

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);
@@ -472,9 +477,9 @@ class DiscreteSearchSolution {
 };
 
 class DiscreteSearch {
-  static DiscreteSearch FromFactorGraph(const gtsam::DiscreteFactorGraph& factorGraph,
+  static gtsam::DiscreteSearch FromFactorGraph(
-                                        const gtsam::Ordering& ordering,
+      const gtsam::DiscreteFactorGraph& factorGraph,
-                                        bool buildJunctionTree = false);
+      const gtsam::Ordering& ordering, bool buildJunctionTree = false);
 
   DiscreteSearch(const gtsam::DiscreteEliminationTree& etree);
   DiscreteSearch(const gtsam::DiscreteJunctionTree& junctionTree);

gtsam/hybrid/hybrid.i

@@ -152,7 +152,11 @@ class HybridBayesNet {
   gtsam::HybridGaussianFactorGraph toFactorGraph(
       const gtsam::VectorValues& measurements) const;
 
+  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;
 

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,
@@ -28,6 +29,30 @@ 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.")
+    parser.add_argument(
+        "--plot_hypotheses",
+        "-p",
+        action="store_true",
+        help="Plot all hypotheses. NOTE: This is exponential, use with caution."
+    )
     return parser.parse_args()
 
 
@@ -39,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()
 
 
@@ -60,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
 
@@ -76,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."""
@@ -84,7 +112,67 @@ class City10000Dataset:
         if line:
             return self.parse_line(line)
         else:
-            return None, None
+            return None, None, None
+
+
+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="",
+                     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.
+        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".
+    """
+    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, prob) in enumerate(all_results):
+        ax = axes[i]
+        ax.axis('equal')
+        ax.axis((-75.0, 100.0, -75.0, 75.0))
+
+        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.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.5,
+             0.015,
+             s=text,
+             wrap=True,
+             horizontalalignment='center',
+             fontsize=12)
+
+    fig.savefig(filename, format="svg")
 
 
 class Experiment:
@@ -93,10 +181,11 @@ class Experiment:
     def __init__(self,
                  filename: str,
                  marginal_threshold: float = 0.9999,
-                 max_loop_count: int = 8000,
+                 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
@@ -108,6 +197,8 @@ class Experiment:
         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):
         """
@@ -147,7 +238,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()
@@ -191,7 +282,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]
@@ -200,6 +291,7 @@ 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[0]
             if key_s == key_t - 1:
                 # Odometry factor
@@ -224,9 +316,15 @@ class Experiment:
                     self.initial_.atPose2(X(key_s)) * odom_pose)
             else:
                 # Loop closure
+                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}")
                 self.new_factors_.push_back(loop_factor)
@@ -240,7 +338,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 +369,85 @@ class Experiment:
         total_time = end_time - start_time
         print(f"Total time: {total_time} seconds")
 
+        # self.save_results(result, key_t + 1, time_list)
+
+        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, index)
+
+    def plot_all_hypotheses(self, discrete_keys, num_poses, num_iters=0):
+        """Plot all possible hypotheses."""
+
+        # Get ground truth
+        gt = np.loadtxt(gtsam.findExampleDataFile("ISAM2_GT_city10000.txt"),
+                        delimiter=" ")
+
+        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))
+        fixed_values_str = " ".join(
+            f"{gtsam.DefaultKeyFormatter(k)}:{v}"
+            for k, v in self.smoother_.fixedValues().items())
+
+        all_assignments = gtsam.cartesianProduct(dkeys)
+
+        all_results = []
+        for assignment in 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))
+
+            poses = np.zeros((num_poses, 3))
+            for i in range(num_poses):
+                pose = result.atPose2(X(i))
+                poses[i] = np.asarray((pose.x(), pose.y(), pose.theta()))
+
+            assignment_string = " ".join([
+                f"{gtsam.DefaultKeyFormatter(k)}={v}"
+                for k, v in assignment.items()
+            ])
+
+            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,
+                         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."""
         # 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)
@@ -312,7 +487,11 @@ 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,
+                            plot_hypotheses=args.plot_hypotheses)
     experiment.run()