diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index fc730f0c9..ea071a020 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -135,6 +135,28 @@ class HybridFactorGraph : public FactorGraph { push_hybrid(p); } } + + /// Get all the discrete keys in the factor graph. + const KeySet allDiscreteKeys() const { + KeySet discrete_keys; + for (auto& factor : factors_) { + for (const DiscreteKey& k : factor->discreteKeys()) { + discrete_keys.insert(k.first); + } + } + return discrete_keys; + } + + /// Get all the continuous keys in the factor graph. + const KeySet allContinuousKeys() const { + KeySet keys; + for (auto& factor : factors_) { + for (const Key& key : factor->continuousKeys()) { + keys.insert(key); + } + } + return keys; + } }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 09b592bd6..c08e774f2 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -404,31 +404,9 @@ void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } -/* ************************************************************************ */ -const KeySet HybridGaussianFactorGraph::getDiscreteKeys() const { - KeySet discrete_keys; - for (auto &factor : factors_) { - for (const DiscreteKey &k : factor->discreteKeys()) { - discrete_keys.insert(k.first); - } - } - return discrete_keys; -} - -/* ************************************************************************ */ -const KeySet HybridGaussianFactorGraph::getContinuousKeys() const { - KeySet keys; - for (auto &factor : factors_) { - for (const Key &key : factor->continuousKeys()) { - keys.insert(key); - } - } - return keys; -} - /* ************************************************************************ */ const Ordering HybridGaussianFactorGraph::getHybridOrdering() const { - KeySet discrete_keys = getDiscreteKeys(); + KeySet discrete_keys = allDiscreteKeys(); for (auto &factor : factors_) { for (const DiscreteKey &k : factor->discreteKeys()) { discrete_keys.insert(k.first); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index ad5cde09b..bd24cdeaa 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -161,12 +161,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph } } - /// Get all the discrete keys in the factor graph. - const KeySet getDiscreteKeys() const; - - /// Get all the continuous keys in the factor graph. - const KeySet getContinuousKeys() const; - /** * @brief Return a Colamd constrained ordering where the discrete keys are * eliminated after the continuous keys.