From bbceb7a3057e1f41f706d81a1dd8df2329916b2f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 25 Mar 2025 09:48:16 -0400
Subject: [PATCH] optionally provide ordering for HybridSmoother::relinearize

---
 gtsam/hybrid/HybridSmoother.cpp | 10 ++++++++--
 gtsam/hybrid/HybridSmoother.h   | 10 +++++++---
 gtsam/hybrid/hybrid.i           |  2 +-
 3 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
index 4beaf6474..1133f645e 100644
--- a/gtsam/hybrid/HybridSmoother.cpp
+++ b/gtsam/hybrid/HybridSmoother.cpp
@@ -292,13 +292,19 @@ HybridValues HybridSmoother::optimize() const {
 }
 
 /* ************************************************************************* */
-void HybridSmoother::relinearize() {
+void HybridSmoother::relinearize(const std::optional<Ordering> givenOrdering) {
   allFactors_ = allFactors_.restrict(fixedValues_);
   HybridGaussianFactorGraph::shared_ptr linearized =
       allFactors_.linearize(linearizationPoint_);
-  HybridBayesNet::shared_ptr bayesNet = linearized->eliminateSequential();
+
+  // Compute ordering if not given
+  Ordering ordering = this->maybeComputeOrdering(*linearized, givenOrdering);
+
+  HybridBayesNet::shared_ptr bayesNet =
+      linearized->eliminateSequential(ordering);
   HybridValues delta = bayesNet->optimize();
   linearizationPoint_ = linearizationPoint_.retract(delta.continuous());
+
   reInitialize(*bayesNet);
 }
 
diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h
index b45327258..77b809a44 100644
--- a/gtsam/hybrid/HybridSmoother.h
+++ b/gtsam/hybrid/HybridSmoother.h
@@ -126,9 +126,13 @@ class GTSAM_EXPORT HybridSmoother {
   /// Optimize the hybrid Bayes Net, taking into accound fixed values.
   HybridValues optimize() const;
 
-  /// Relinearize the nonlinear factor graph
-  /// with the latest linearization point.
-  void relinearize();
+  /**
+   * @brief Relinearize the nonlinear factor graph with
+   * the latest stored linearization point.
+   *
+   * @param givenOrdering An optional elimination ordering.
+   */
+  void relinearize(const std::optional<Ordering> givenOrdering = {});
 
   /// Return the current linearization point.
   Values linearizationPoint() const;
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index f76de20be..e231714cb 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -288,7 +288,7 @@ class HybridSmoother {
       std::optional<size_t> maxNrLeaves = std::nullopt,
       const std::optional<gtsam::Ordering> given_ordering = std::nullopt);
 
-  void relinearize();
+  void relinearize(const std::optional<gtsam::Ordering> givenOrdering);
 
   gtsam::Values linearizationPoint() const;
   gtsam::HybridNonlinearFactorGraph allFactors() const;