From 6f89d457ca89494dcfba44b3fd7089d4e06ae730 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 25 Oct 2022 17:50:10 -0400
Subject: [PATCH] Add GetOrdering method that can be re-used

---
 gtsam/hybrid/HybridGaussianISAM.cpp | 53 +++++++++++++++++------------
 gtsam/hybrid/HybridGaussianISAM.h   | 11 ++++++
 2 files changed, 42 insertions(+), 22 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp
index de87dd92f..aa6b3f266 100644
--- a/gtsam/hybrid/HybridGaussianISAM.cpp
+++ b/gtsam/hybrid/HybridGaussianISAM.cpp
@@ -38,6 +38,35 @@ HybridGaussianISAM::HybridGaussianISAM() {}
 HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
     : Base(bayesTree) {}
 
+/* ************************************************************************* */
+Ordering HybridGaussianISAM::GetOrdering(
+    HybridGaussianFactorGraph& factors,
+    const HybridGaussianFactorGraph& newFactors) {
+  // Get all the discrete keys from the factors
+  KeySet allDiscrete = factors.discreteKeys();
+
+  // 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(factors);
+
+  // Get an ordering where the new keys are eliminated last
+  Ordering ordering = Ordering::ColamdConstrainedLast(
+      index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
+      true);
+  return ordering;
+}
+
 /* ************************************************************************* */
 void HybridGaussianISAM::updateInternal(
     const HybridGaussianFactorGraph& newFactors,
@@ -54,7 +83,7 @@ void HybridGaussianISAM::updateInternal(
   }
 
   // Add the removed top and the new factors
-  FactorGraphType factors;
+  HybridGaussianFactorGraph factors;
   factors += bn;
   factors += newFactors;
 
@@ -63,32 +92,12 @@ void HybridGaussianISAM::updateInternal(
     factors += boost::make_shared<BayesTreeOrphanWrapper<Node>>(orphan);
   }
 
-  // Get all the discrete keys from the factors
-  KeySet allDiscrete = factors.discreteKeys();
-
-  // Create KeyVector with continuous keys followed by discrete keys.
-  KeyVector newKeysDiscreteLast;
-  // 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));
-
-  // Get an ordering where the new keys are eliminated last
   const VariableIndex index(factors);
-
   Ordering elimination_ordering;
   if (ordering) {
     elimination_ordering = *ordering;
   } else {
-    elimination_ordering = Ordering::ColamdConstrainedLast(
-        index,
-        KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
-        true);
+    elimination_ordering = GetOrdering(factors, newFactors);
   }
 
   // eliminate all factors (top, added, orphans) into a new Bayes tree
diff --git a/gtsam/hybrid/HybridGaussianISAM.h b/gtsam/hybrid/HybridGaussianISAM.h
index a6a82b3e8..bc5f04e0f 100644
--- a/gtsam/hybrid/HybridGaussianISAM.h
+++ b/gtsam/hybrid/HybridGaussianISAM.h
@@ -67,6 +67,17 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
               const boost::optional<Ordering>& ordering = boost::none,
               const HybridBayesTree::Eliminate& function =
                   HybridBayesTree::EliminationTraitsType::DefaultEliminate);
+
+  /**
+   * @brief Helper method to get an ordering given the existing factors and any
+   * new factors added.
+   *
+   * @param factors The existing factors in the BayesTree.
+   * @param newFactors New factors added during the update step.
+   * @return Ordering
+   */
+  static Ordering GetOrdering(HybridGaussianFactorGraph& factors,
+                              const HybridGaussianFactorGraph& newFactors);
 };
 
 /// traits