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

Refactor elimination setup to not use C declaration style

release/4.3a0
Frank Dellaert 2022-12-25 19:21:34 -05:00
parent 4ad482aa70
commit a7573e8e6f
1 changed files with 33 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -344,18 +344,20 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
// However this is also the case with iSAM2, so no pressure :)
// PREPROCESS: Identify the nature of the current elimination
std::unordered_map<Key, DiscreteKey> mapFromKeyToDiscreteKey;
std::set<DiscreteKey> discreteSeparatorSet;
std::set<DiscreteKey> discreteFrontals;
// First, identify the separator keys, i.e. all keys that are not frontal.
KeySet separatorKeys;
KeySet allContinuousKeys;
KeySet continuousFrontals;
KeySet continuousSeparator;
// This initializes separatorKeys and mapFromKeyToDiscreteKey
for (auto &&factor : factors) {
separatorKeys.insert(factor->begin(), factor->end());
}
// remove frontals from separator
for (auto &k : frontalKeys) {
separatorKeys.erase(k);
}
// Build a map from keys to DiscreteKeys
std::unordered_map<Key, DiscreteKey> mapFromKeyToDiscreteKey;
for (auto &&factor : factors) {
if (!factor->isContinuous()) {
for (auto &k : factor->discreteKeys()) {
mapFromKeyToDiscreteKey[k.first] = k;
@ -363,49 +365,50 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
}
}
// remove frontals from separator
for (auto &k : frontalKeys) {
separatorKeys.erase(k);
}
// Fill in discrete frontals and continuous frontals for the end result
// Fill in discrete frontals and continuous frontals.
std::set<DiscreteKey> discreteFrontals;
KeySet continuousFrontals;
for (auto &k : frontalKeys) {
if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k));
} else {
continuousFrontals.insert(k);
allContinuousKeys.insert(k);
}
}
// Fill in discrete frontals and continuous frontals for the end result
// Fill in discrete discrete separator keys and continuous separator keys.
std::set<DiscreteKey> discreteSeparatorSet;
KeySet continuousSeparator;
for (auto &k : separatorKeys) {
if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k));
} else {
continuousSeparator.insert(k);
allContinuousKeys.insert(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
// Case 1: we are only dealing with continuous
if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) {
return continuousElimination(factors, frontalKeys);
}
// Case 2: we are only dealing with discrete
if (allContinuousKeys.empty()) {
if (discrete_only) {
// Case 1: we are only dealing with discrete
return discreteElimination(factors, frontalKeys);
}
} else {
// Case 2: we are only dealing with continuous
if (mapFromKeyToDiscreteKey.empty()) {
return continuousElimination(factors, frontalKeys);
} else {
// Case 3: We are now in the hybrid land!
#ifdef HYBRID_TIMING
tictoc_reset_();
tictoc_reset_();
#endif
// Case 3: We are now in the hybrid land!
return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparatorSet);
return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparatorSet);
}
}
}
/* ************************************************************************ */