Merge pull request #2019 from borglab/city10000-py

borglab · Feb 9, 2025 · 82fcedf · 82fcedf
2 parents 2998d98 + 2d18a0c
commit 82fcedf
Show file tree

Hide file tree

Showing 7 changed files with 485 additions and 9 deletions.
diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
@@ -24,6 +24,16 @@
 // #define DEBUG_SMOOTHER
 namespace gtsam {
 
+/* ************************************************************************* */
+void HybridSmoother::reInitialize(HybridBayesNet &&hybridBayesNet) {
+  hybridBayesNet_ = std::move(hybridBayesNet);
+}
+
+/* ************************************************************************* */
+void HybridSmoother::reInitialize(HybridBayesNet &hybridBayesNet) {
+  this->reInitialize(std::move(hybridBayesNet));
+}
+
 /* ************************************************************************* */
 Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
                                      const KeySet &lastKeysToEliminate) {
@@ -78,9 +88,11 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
   // If no ordering provided, then we compute one
   if (!given_ordering.has_value()) {
     // Get the keys from the new factors
-    KeySet continuousKeysToInclude;  // Scheme 1: empty, 15sec/2000, 64sec/3000 (69s without TF)
-    // continuousKeysToInclude = newFactors.keys();  // Scheme 2: all, 8sec/2000, 160sec/3000
-    // continuousKeysToInclude = updatedGraph.keys();  // Scheme 3: all, stopped after 80sec/2000
+    KeySet continuousKeysToInclude;  // Scheme 1: empty, 15sec/2000, 64sec/3000
+                                     // (69s without TF)
+    // continuousKeysToInclude = newFactors.keys();  // Scheme 2: all,
+    // 8sec/2000, 160sec/3000 continuousKeysToInclude = updatedGraph.keys();  //
+    // Scheme 3: all, stopped after 80sec/2000
 
     // Since updatedGraph now has all the connected conditionals,
     // we can get the correct ordering.

diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h
@@ -49,9 +49,13 @@ class GTSAM_EXPORT HybridSmoother {
   /**
    * Re-initialize the smoother from a new hybrid Bayes Net.
    */
-  void reInitialize(HybridBayesNet&& hybridBayesNet) {
-    hybridBayesNet_ = std::move(hybridBayesNet);
-  }
+  void reInitialize(HybridBayesNet&& hybridBayesNet);
+
+  /**
+   * Re-initialize the smoother from
+   * a new hybrid Bayes Net (non rvalue version).
+   */
+  void reInitialize(HybridBayesNet& hybridBayesNet);
 
   /**
    * Given new factors, perform an incremental update.

diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
@@ -227,14 +227,20 @@ class HybridNonlinearFactorGraph {
   void push_back(gtsam::HybridFactor* factor);
   void push_back(gtsam::NonlinearFactor* factor);
   void push_back(gtsam::DiscreteFactor* factor);
+  void push_back(const gtsam::HybridNonlinearFactorGraph& graph);
+  // TODO(Varun) Wrap add() methods
+
   gtsam::HybridGaussianFactorGraph linearize(
       const gtsam::Values& continuousValues) const;
 
   bool empty() const;
   void remove(size_t i);
   size_t size() const;
+  void resize(size_t size);
   gtsam::KeySet keys() const;
   const gtsam::HybridFactor* at(size_t i) const;
+  gtsam::HybridNonlinearFactorGraph restrict(
+      const gtsam::DiscreteValues& assignment) const;
 
   void print(string s = "HybridNonlinearFactorGraph\n",
              const gtsam::KeyFormatter& keyFormatter =
@@ -243,9 +249,8 @@ class HybridNonlinearFactorGraph {
 
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 class HybridNonlinearFactor : gtsam::HybridFactor {
-  HybridNonlinearFactor(
-      const gtsam::DiscreteKey& discreteKey,
-      const std::vector<gtsam::NoiseModelFactor*>& factors);
+  HybridNonlinearFactor(const gtsam::DiscreteKey& discreteKey,
+                        const std::vector<gtsam::NoiseModelFactor*>& factors);
 
   HybridNonlinearFactor(
       const gtsam::DiscreteKey& discreteKey,
@@ -266,4 +271,29 @@ class HybridNonlinearFactor : gtsam::HybridFactor {
                  gtsam::DefaultKeyFormatter) const;
 };
 
+#include <gtsam/hybrid/HybridSmoother.h>
+class HybridSmoother {
+  HybridSmoother(const std::optional<double> marginalThreshold = std::nullopt);
+
+  const gtsam::DiscreteValues& fixedValues() const;
+  void reInitialize(gtsam::HybridBayesNet& hybridBayesNet);
+
+  void update(
+      const gtsam::HybridGaussianFactorGraph& graph,
+      std::optional<size_t> maxNrLeaves = std::nullopt,
+      const std::optional<gtsam::Ordering> given_ordering = std::nullopt);
+
+  gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors,
+                              const gtsam::KeySet& newFactorKeys);
+
+  std::pair<gtsam::HybridGaussianFactorGraph, gtsam::HybridBayesNet>
+  addConditionals(const gtsam::HybridGaussianFactorGraph& graph,
+                  const gtsam::HybridBayesNet& hybridBayesNet) const;
+
+  gtsam::HybridGaussianConditional* gaussianMixture(size_t index) const;
+
+  const gtsam::HybridBayesNet& hybridBayesNet() const;
+  gtsam::HybridValues optimize() const;
+};
+
 }  // namespace gtsam
diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
@@ -32,6 +32,8 @@ virtual class Gaussian : gtsam::noiseModel::Base {
   gtsam::Vector unwhiten(gtsam::Vector v) const;
   gtsam::Matrix Whiten(gtsam::Matrix H) const;
 
+  double negLogConstant() const;
+
   // enabling serialization functionality
   void serializable() const;
 };

diff --git a/python/gtsam/examples/.gitignore b/python/gtsam/examples/.gitignore
@@ -0,0 +1 @@
+*.txt