diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index f6b713a76..3be438e43 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -58,6 +58,21 @@ namespace gtsam { /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph: template class EliminateableFactorGraph; +/* ************************************************************************ */ +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(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 HybridGaussianFactorGraph::error( const VectorValues &continuousValues) const { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 144d144bb..44ef7d784 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -53,6 +53,15 @@ GTSAM_EXPORT std::pair, 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 { @@ -74,6 +83,12 @@ struct EliminationTraits { DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { return EliminateHybrid(factors, keys); } + /// The default ordering generation function + static Ordering DefaultOrderingFunc( + const FactorGraphType& graph, + boost::optional 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.