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

simplify HybridEliminate

release/4.3a0
Varun Agrawal 2023-07-06 23:07:38 -04:00
parent 4e902fc8a7
commit 4e13fb717b
1 changed files with 45 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -348,64 +348,68 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
// When the number of assignments is large we may encounter stack overflows. // When the number of assignments is large we may encounter stack overflows.
// However this is also the case with iSAM2, so no pressure :) // However this is also the case with iSAM2, so no pressure :)
// PREPROCESS: Identify the nature of the current elimination // Check the factors:
// TODO(dellaert): just check the factors:
// 1. if all factors are discrete, then we can do discrete elimination: // 1. if all factors are discrete, then we can do discrete elimination:
// 2. if all factors are continuous, then we can do continuous elimination: // 2. if all factors are continuous, then we can do continuous elimination:
// 3. if not, we do hybrid elimination: // 3. if not, we do hybrid elimination:
// First, identify the separator keys, i.e. all keys that are not frontal. bool only_discrete = true, only_continuous = true;
KeySet separatorKeys;
for (auto &&factor : factors) { for (auto &&factor : factors) {
separatorKeys.insert(factor->begin(), factor->end()); if (auto hybrid_factor = std::dynamic_pointer_cast<HybridFactor>(factor)) {
} if (hybrid_factor->isDiscrete()) {
// remove frontals from separator only_continuous = false;
for (auto &k : frontalKeys) { } else if (hybrid_factor->isContinuous()) {
separatorKeys.erase(k); only_discrete = false;
} } else if (hybrid_factor->isHybrid()) {
only_continuous = false;
// Build a map from keys to DiscreteKeys only_discrete = false;
auto mapFromKeyToDiscreteKey = factors.discreteKeyMap(); }
} else if (auto cont_factor =
// Fill in discrete frontals and continuous frontals. std::dynamic_pointer_cast<GaussianFactor>(factor)) {
std::set<DiscreteKey> discreteFrontals; only_discrete = false;
KeySet continuousFrontals; } else if (auto discrete_factor =
for (auto &k : frontalKeys) { std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) { only_continuous = false;
discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k));
} else {
continuousFrontals.insert(k);
} }
} }
// Fill in discrete discrete separator keys and continuous separator keys.
std::set<DiscreteKey> discreteSeparatorSet;
KeyVector continuousSeparator;
for (auto &k : separatorKeys) {
if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k));
} else {
continuousSeparator.push_back(k);
}
}
// Check if we have any continuous keys:
const bool discrete_only =
continuousFrontals.empty() && continuousSeparator.empty();
// NOTE: We should really defer the product here because of pruning // NOTE: We should really defer the product here because of pruning
if (discrete_only) { if (only_discrete) {
// Case 1: we are only dealing with discrete // Case 1: we are only dealing with discrete
return discreteElimination(factors, frontalKeys); return discreteElimination(factors, frontalKeys);
} else if (mapFromKeyToDiscreteKey.empty()) { } else if (only_continuous) {
// Case 2: we are only dealing with continuous // Case 2: we are only dealing with continuous
return continuousElimination(factors, frontalKeys); return continuousElimination(factors, frontalKeys);
} else { } else {
// Case 3: We are now in the hybrid land! // Case 3: We are now in the hybrid land!
KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end());
// Find all the keys in the set of continuous keys
// which are not in the frontal keys. This is our continuous separator.
KeyVector continuousSeparator;
auto continuousKeySet = factors.continuousKeySet();
std::set_difference(
continuousKeySet.begin(), continuousKeySet.end(),
frontalKeysSet.begin(), frontalKeysSet.end(),
std::inserter(continuousSeparator, continuousSeparator.begin()));
// Similarly for the discrete separator.
KeySet discreteSeparatorSet;
std::set<DiscreteKey> discreteSeparator;
auto discreteKeySet = factors.discreteKeySet();
std::set_difference(
discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(),
frontalKeysSet.end(),
std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin()));
// Convert from set of keys to set of DiscreteKeys
auto discreteKeyMap = factors.discreteKeyMap();
for (auto key : discreteSeparatorSet) {
discreteSeparator.insert(discreteKeyMap.at(key));
}
return hybridElimination(factors, frontalKeys, continuousSeparator, return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparatorSet); discreteSeparator);
} }
} }