HybridSmoother based on BayesNet

gtsam/hybrid/HybridSmoother.cpp

@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    HybridSmoother.cpp
+ * @brief   An incremental smoother for hybrid factor graphs
+ * @author  Varun Agrawal
+ * @date    October 2022
+ */
+
+#include <gtsam/hybrid/HybridSmoother.h>
+
+#include <algorithm>
+#include <unordered_set>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void HybridSmoother::update(HybridGaussianFactorGraph graph,
+                            const Ordering &ordering,
+                            boost::optional<size_t> maxNrLeaves) {
+  // Add the necessary conditionals from the previous timestep(s).
+  std::tie(graph, hybridBayesNet_) =
+      addConditionals(graph, hybridBayesNet_, ordering);
+
+  // Eliminate.
+  auto bayesNetFragment = graph.eliminateSequential(ordering);
+
+  /// Prune
+  if (maxNrLeaves) {
+    // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in
+    // all the conditionals with the same keys in bayesNetFragment.
+    HybridBayesNet prunedBayesNetFragment =
+        bayesNetFragment->prune(*maxNrLeaves);
+    // Set the bayes net fragment to the pruned version
+    bayesNetFragment =
+        boost::make_shared<HybridBayesNet>(prunedBayesNetFragment);
+  }
+
+  // Add the partial bayes net to the posterior bayes net.
+  hybridBayesNet_.push_back<HybridBayesNet>(*bayesNetFragment);
+
+  tictoc_print_();
+}
+
+/* ************************************************************************* */
+std::pair<HybridGaussianFactorGraph, HybridBayesNet>
+HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
+                                const HybridBayesNet &originalHybridBayesNet,
+                                const Ordering &ordering) const {
+  HybridGaussianFactorGraph graph(originalGraph);
+  HybridBayesNet hybridBayesNet(originalHybridBayesNet);
+
+  // If we are not at the first iteration, means we have conditionals to add.
+  if (!hybridBayesNet.empty()) {
+    // We add all relevant conditional mixtures on the last continuous variable
+    // in the previous `hybridBayesNet` to the graph
+
+    // Conditionals to remove from the bayes net
+    // since the conditional will be updated.
+    std::vector<HybridConditional::shared_ptr> conditionals_to_erase;
+
+    // New conditionals to add to the graph
+    gtsam::HybridBayesNet newConditionals;
+
+    // NOTE(Varun) Using a for-range loop doesn't work since some of the
+    // conditionals are invalid pointers
+    for (size_t i = 0; i < hybridBayesNet.size(); i++) {
+      auto conditional = hybridBayesNet.at(i);
+
+      for (auto &key : conditional->frontals()) {
+        if (std::find(ordering.begin(), ordering.end(), key) !=
+            ordering.end()) {
+          newConditionals.push_back(conditional);
+          conditionals_to_erase.push_back(conditional);
+
+          break;
+        }
+      }
+    }
+    // Remove conditionals at the end so we don't affect the order in the
+    // original bayes net.
+    for (auto &&conditional : conditionals_to_erase) {
+      auto it = find(hybridBayesNet.begin(), hybridBayesNet.end(), conditional);
+      hybridBayesNet.erase(it);
+    }
+
+    graph.push_back(newConditionals);
+    // newConditionals.print("\n\n\nNew Conditionals to add back");
+  }
+  return {graph, hybridBayesNet};
+}
+
+/* ************************************************************************* */
+GaussianMixture::shared_ptr HybridSmoother::gaussianMixture(
+    size_t index) const {
+  return boost::dynamic_pointer_cast<GaussianMixture>(
+      hybridBayesNet_.at(index));
+}
+
+/* ************************************************************************* */
+const HybridBayesNet &HybridSmoother::hybridBayesNet() const {
+  return hybridBayesNet_;
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridSmoother.h

@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    HybridSmoother.h
+ * @brief   An incremental smoother for hybrid factor graphs
+ * @author  Varun Agrawal
+ * @date    October 2022
+ */
+
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+
+namespace gtsam {
+
+class HybridSmoother {
+ private:
+  HybridBayesNet hybridBayesNet_;
+  HybridGaussianFactorGraph remainingFactorGraph_;
+
+ public:
+  /**
+   * Given new factors, perform an incremental update.
+   * The relevant densities in the `hybridBayesNet` will be added to the input
+   * graph (fragment), and then eliminated according to the `ordering`
+   * presented. The remaining factor graph contains Gaussian mixture factors
+   * that are not connected to the variables in the ordering, or a single
+   * discrete factor on all discrete keys, plus all discrete factors in the
+   * original graph.
+   *
+   * \note If maxComponents is given, we look at the discrete factor resulting
+   * from this elimination, and prune it and the Gaussian components
+   * corresponding to the pruned choices.
+   *
+   * @param graph The new factors, should be linear only
+   * @param ordering The ordering for elimination, only continuous vars are
+   * allowed
+   * @param maxNrLeaves The maximum number of leaves in the new discrete factor,
+   * if applicable
+   */
+  void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
+              boost::optional<size_t> maxNrLeaves = boost::none);
+
+  /**
+   * @brief Add conditionals from previous timestep as part of liquefication.
+   *
+   * @param graph The new factor graph for the current time step.
+   * @param hybridBayesNet The hybrid bayes net containing all conditionals so
+   * far.
+   * @param ordering The elimination ordering.
+   * @return std::pair<HybridGaussianFactorGraph, HybridBayesNet>
+   */
+  std::pair<HybridGaussianFactorGraph, HybridBayesNet> addConditionals(
+      const HybridGaussianFactorGraph& graph,
+      const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const;
+
+  /// Get the Gaussian Mixture from the Bayes Net posterior at `index`.
+  GaussianMixture::shared_ptr gaussianMixture(size_t index) const;
+
+  /// Return the Bayes Net posterior.
+  const HybridBayesNet& hybridBayesNet() const;
+};
+
+};  // namespace gtsam