Make HybridOrdering a function and use it for Hybrid DefaultOrderingFunc

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -58,6 +58,21 @@ namespace gtsam {
 /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph:
 template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
 
+/* ************************************************************************ */
+const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) {
+  KeySet discrete_keys = graph.discreteKeys();
+  for (auto &factor : graph) {
+    for (const DiscreteKey &k : factor->discreteKeys()) {
+      discrete_keys.insert(k.first);
+    }
+  }
+
+  const VariableIndex index(graph);
+  Ordering ordering = Ordering::ColamdConstrainedLast(
+      index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true);
+  return ordering;
+}
+
 /* ************************************************************************ */
 static GaussianFactorGraphTree addGaussian(
     const GaussianFactorGraphTree &gfgTree,
@@ -448,21 +463,6 @@ void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
   FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
 }
 
-/* ************************************************************************ */
-const Ordering HybridGaussianFactorGraph::getHybridOrdering() const {
-  KeySet discrete_keys = discreteKeys();
-  for (auto &factor : factors_) {
-    for (const DiscreteKey &k : factor->discreteKeys()) {
-      discrete_keys.insert(k.first);
-    }
-  }
-
-  const VariableIndex index(factors_);
-  Ordering ordering = Ordering::ColamdConstrainedLast(
-      index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true);
-  return ordering;
-}
-
 /* ************************************************************************ */
 AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
     const VectorValues &continuousValues) const {

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -53,6 +53,15 @@ GTSAM_EXPORT
 std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
 EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
 
+/**
+ * @brief Return a Colamd constrained ordering where the discrete keys are
+ * eliminated after the continuous keys.
+ *
+ * @return const Ordering
+ */
+GTSAM_EXPORT const Ordering
+HybridOrdering(const HybridGaussianFactorGraph& graph);
+
 /* ************************************************************************* */
 template <>
 struct EliminationTraits<HybridGaussianFactorGraph> {
@@ -74,6 +83,12 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
   DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
     return EliminateHybrid(factors, keys);
   }
+  /// The default ordering generation function
+  static Ordering DefaultOrderingFunc(
+      const FactorGraphType& graph,
+      boost::optional<const VariableIndex&> variableIndex) {
+    return HybridOrdering(graph);
+  }
 };
 
 /**
@@ -228,14 +243,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
    */
   double probPrime(const HybridValues& values) const;
 
-  /**
-   * @brief Return a Colamd constrained ordering where the discrete keys are
-   * eliminated after the continuous keys.
-   *
-   * @return const Ordering
-   */
-  const Ordering getHybridOrdering() const;
-
   /**
    * @brief Create a decision tree of factor graphs out of this hybrid factor
    * graph.