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..46d865a0b 100644 --- a/gtsam/discrete/DiscreteValues.h +++ b/gtsam/discrete/DiscreteValues.h @@ -188,6 +188,11 @@ inline std::vector 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, diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 5e4d8d22d..74de7a922 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 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, - 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); diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 1e6f7bf25..5e93c85a9 100644 --- a/gtsam/hybrid/hybrid.i +++ b/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; diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py index 5a9ef9585..e1f4b9d05 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, @@ -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,8 +316,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}") @@ -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()