From e1f5a5155a9556812a56bc7340ee3efb0818942c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Mar 2025 16:55:34 -0400
Subject: [PATCH] update the HybridCity10000 script

---
 gtsam/hybrid/HybridSmoother.cpp          |  5 +++++
 gtsam/hybrid/HybridSmoother.h            |  3 +++
 gtsam/hybrid/hybrid.i                    |  1 +
 python/gtsam/examples/HybridCity10000.py | 17 +++++------------
 4 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
index aa3bf0657..4beaf6474 100644
--- a/gtsam/hybrid/HybridSmoother.cpp
+++ b/gtsam/hybrid/HybridSmoother.cpp
@@ -307,4 +307,9 @@ Values HybridSmoother::linearizationPoint() const {
   return linearizationPoint_;
 }
 
+/* ************************************************************************* */
+HybridNonlinearFactorGraph HybridSmoother::allFactors() const {
+  return allFactors_;
+}
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h
index 7ef7f4ebb..b45327258 100644
--- a/gtsam/hybrid/HybridSmoother.h
+++ b/gtsam/hybrid/HybridSmoother.h
@@ -133,6 +133,9 @@ class GTSAM_EXPORT HybridSmoother {
   /// Return the current linearization point.
   Values linearizationPoint() const;
 
+  /// Return all the recorded nonlinear factors
+  HybridNonlinearFactorGraph allFactors() const;
+
  private:
   /// Helper to compute the ordering if ordering is not given.
   Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph,
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index fa58caea7..f76de20be 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -291,6 +291,7 @@ class HybridSmoother {
   void relinearize();
 
   gtsam::Values linearizationPoint() const;
+  gtsam::HybridNonlinearFactorGraph allFactors() const;
 
   gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors,
                               const gtsam::KeySet& newFactorKeys);
diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py
index e1f4b9d05..17be1b267 100644
--- a/python/gtsam/examples/HybridCity10000.py
+++ b/python/gtsam/examples/HybridCity10000.py
@@ -194,7 +194,6 @@ class Experiment:
 
         self.smoother_ = HybridSmoother(marginal_threshold)
         self.new_factors_ = HybridNonlinearFactorGraph()
-        self.all_factors_ = HybridNonlinearFactorGraph()
         self.initial_ = Values()
 
         self.plot_hypotheses = plot_hypotheses
@@ -231,24 +230,18 @@ class Experiment:
         """Perform smoother update and optimize the graph."""
         print(f"Smoother update: {self.new_factors_.size()}")
         before_update = time.time()
-        linearized = self.new_factors_.linearize(self.initial_)
-        self.smoother_.update(linearized, max_num_hypotheses)
-        self.all_factors_.push_back(self.new_factors_)
+        self.smoother_.update(self.new_factors_, self.initial_,
+                              max_num_hypotheses)
         self.new_factors_.resize(0)
         after_update = time.time()
         return after_update - before_update
 
     def reinitialize(self) -> float:
         """Re-linearize, solve ALL, and re-initialize smoother."""
-        print(f"================= Re-Initialize: {self.all_factors_.size()}")
+        print(f"================= Re-Initialize: {self.smoother_.allFactors().size()}")
         before_update = time.time()
-        self.all_factors_ = self.all_factors_.restrict(
-            self.smoother_.fixedValues())
-        linearized = self.all_factors_.linearize(self.initial_)
-        bayesNet = linearized.eliminateSequential()
-        delta: HybridValues = bayesNet.optimize()
-        self.initial_ = self.initial_.retract(delta.continuous())
-        self.smoother_.reInitialize(bayesNet)
+        self.smoother_.relinearize()
+        self.initial_ = self.smoother_.linearizationPoint()
         after_update = time.time()
         print(f"Took {after_update - before_update} seconds.")
         return after_update - before_update