12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

move renamed allDiscreteKeys and allContinuousKeys to HybridFactorGraph

release/4.3a0
Varun Agrawal 2022-09-17 08:04:55 -04:00
parent aebcde99e2
commit 9ef5c184ec
3 changed files with 23 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/hybrid/HybridFactorGraph.h
View File

@ -135,6 +135,28 @@ class HybridFactorGraph : public FactorGraph<HybridFactor> {
push_hybrid(p); 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 } // namespace gtsam

24
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -404,31 +404,9 @@ void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor)); FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(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 { const Ordering HybridGaussianFactorGraph::getHybridOrdering() const {
KeySet discrete_keys = getDiscreteKeys(); KeySet discrete_keys = allDiscreteKeys();
for (auto &factor : factors_) { for (auto &factor : factors_) {
for (const DiscreteKey &k : factor->discreteKeys()) { for (const DiscreteKey &k : factor->discreteKeys()) {
discrete_keys.insert(k.first); discrete_keys.insert(k.first);

6
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -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 * @brief Return a Colamd constrained ordering where the discrete keys are
* eliminated after the continuous keys. * eliminated after the continuous keys.