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

improve docs when computing discreteSeparator

release/4.3a0
Varun Agrawal 2024-09-03 15:27:33 -04:00
parent 82fc9b9eeb
commit 06ecf00dba
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridFactorGraph.h
View File

@ -66,7 +66,7 @@ class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
/// Get all the discrete keys in the factor graph. /// Get all the discrete keys in the factor graph.
std::set<DiscreteKey> discreteKeys() const; std::set<DiscreteKey> discreteKeys() const;
/// Get all the discrete keys in the factor graph, as a set. /// Get all the discrete keys in the factor graph, as a set of Keys.
KeySet discreteKeySet() const; KeySet discreteKeySet() const;
/// Get a map from Key to corresponding DiscreteKey. /// Get a map from Key to corresponding DiscreteKey.

7
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -330,7 +330,7 @@ static std::shared_ptr<Factor> createDiscreteFactor(
// exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); // exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
// We take negative of the logNormalizationConstant `log(1/k)` // We take negative of the logNormalizationConstant `log(1/k)`
// to get `log(k)`. // to get `log(k)`.
return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); return -factor->error(kEmpty) - conditional->logNormalizationConstant();
}; };
AlgebraicDecisionTree<Key> logProbabilities( AlgebraicDecisionTree<Key> logProbabilities(
@ -523,14 +523,15 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
std::inserter(continuousSeparator, continuousSeparator.begin())); std::inserter(continuousSeparator, continuousSeparator.begin()));
// Similarly for the discrete separator. // Similarly for the discrete separator.
KeySet discreteSeparatorSet;
std::set<DiscreteKey> discreteSeparator;
auto discreteKeySet = factors.discreteKeySet(); auto discreteKeySet = factors.discreteKeySet();
// Use set-difference to get the difference in keys
KeySet discreteSeparatorSet;
std::set_difference( std::set_difference(
discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(), discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(),
frontalKeysSet.end(), frontalKeysSet.end(),
std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin())); std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin()));
// Convert from set of keys to set of DiscreteKeys // Convert from set of keys to set of DiscreteKeys
std::set<DiscreteKey> discreteSeparator;
auto discreteKeyMap = factors.discreteKeyMap(); auto discreteKeyMap = factors.discreteKeyMap();
for (auto key : discreteSeparatorSet) { for (auto key : discreteSeparatorSet) {
discreteSeparator.insert(discreteKeyMap.at(key)); discreteSeparator.insert(discreteKeyMap.at(key));