From 2714dc562550b5c6f0b774f2b215172900ef6dc3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 13 Feb 2023 15:59:30 -0500
Subject: [PATCH] add ordering method for HybridSmoother

---
 gtsam/hybrid/HybridSmoother.cpp | 32 +++++++++++++++++++++++++++++++-
 gtsam/hybrid/HybridSmoother.h   |  2 ++
 2 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
index 35dd5f88b..fcee7833a 100644
--- a/gtsam/hybrid/HybridSmoother.cpp
+++ b/gtsam/hybrid/HybridSmoother.cpp
@@ -23,6 +23,37 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
+Ordering HybridSmoother::getOrdering(
+    const HybridGaussianFactorGraph &newFactors) {
+  HybridGaussianFactorGraph factors(hybridBayesNet());
+  factors += newFactors;
+  // Get all the discrete keys from the factors
+  KeySet allDiscrete = factors.discreteKeySet();
+
+  // Create KeyVector with continuous keys followed by discrete keys.
+  KeyVector newKeysDiscreteLast;
+  const KeySet newFactorKeys = newFactors.keys();
+  // Insert continuous keys first.
+  for (auto &k : newFactorKeys) {
+    if (!allDiscrete.exists(k)) {
+      newKeysDiscreteLast.push_back(k);
+    }
+  }
+
+  // Insert discrete keys at the end
+  std::copy(allDiscrete.begin(), allDiscrete.end(),
+            std::back_inserter(newKeysDiscreteLast));
+
+  const VariableIndex index(newFactors);
+
+  // Get an ordering where the new keys are eliminated last
+  Ordering ordering = Ordering::ColamdConstrainedLast(
+      index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
+      true);
+  return ordering;
+}
+
 /* ************************************************************************* */
 void HybridSmoother::update(HybridGaussianFactorGraph graph,
                             const Ordering &ordering,
@@ -92,7 +123,6 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
     }
 
     graph.push_back(newConditionals);
-    // newConditionals.print("\n\n\nNew Conditionals to add back");
   }
   return {graph, hybridBayesNet};
 }
diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h
index 7e90f9425..9f14a7002 100644
--- a/gtsam/hybrid/HybridSmoother.h
+++ b/gtsam/hybrid/HybridSmoother.h
@@ -50,6 +50,8 @@ class HybridSmoother {
   void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
               boost::optional<size_t> maxNrLeaves = boost::none);
 
+  Ordering getOrdering(const HybridGaussianFactorGraph& newFactors);
+
   /**
    * @brief Add conditionals from previous timestep as part of liquefication.
    *