optionally provide ordering for HybridSmoother::relinearize

Author: varunagrawal

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);
 }
 

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;

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;