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

remove if guards and add timing counters

release/4.3a0
Varun Agrawal 2022-10-25 12:36:15 -04:00
parent 08393314ac
commit 7dec7bb00d
1 changed files with 24 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -214,24 +214,35 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
if (graph.empty()) { if (graph.empty()) {
return {nullptr, nullptr}; return {nullptr, nullptr};
} }
#ifdef HYBRID_TIMING
gttic_(hybrid_eliminate);
#endif
std::pair<boost::shared_ptr<GaussianConditional>, std::pair<boost::shared_ptr<GaussianConditional>,
boost::shared_ptr<GaussianFactor>> boost::shared_ptr<GaussianFactor>>
result = EliminatePreferCholesky(graph, frontalKeys); result = EliminatePreferCholesky(graph, frontalKeys);
if (keysOfEliminated.empty()) { // Initialize the keysOfEliminated to be the keys of the
// Initialize the keysOfEliminated to be the keys of the // eliminated GaussianConditional
// eliminated GaussianConditional keysOfEliminated = result.first->keys();
keysOfEliminated = result.first->keys(); keysOfSeparator = result.second->keys();
}
if (keysOfSeparator.empty()) { #ifdef HYBRID_TIMING
keysOfSeparator = result.second->keys(); gttoc_(hybrid_eliminate);
} #endif
return result; return result;
}; };
// Perform elimination! // Perform elimination!
DecisionTree<Key, EliminationPair> eliminationResults(sum, eliminate); DecisionTree<Key, EliminationPair> eliminationResults(sum, eliminate);
#ifdef HYBRID_TIMING
tictoc_print_();
tictoc_reset_();
#endif
// Separate out decision tree into conditionals and remaining factors. // Separate out decision tree into conditionals and remaining factors.
auto pair = unzip(eliminationResults); auto pair = unzip(eliminationResults);
@ -244,6 +255,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
// If there are no more continuous parents, then we should create here a // If there are no more continuous parents, then we should create here a
// DiscreteFactor, with the error for each discrete choice. // DiscreteFactor, with the error for each discrete choice.
if (keysOfSeparator.empty()) { if (keysOfSeparator.empty()) {
// TODO(Varun) Use the math from the iMHS_Math-1-indexed document
// TODO(Varun) The prob of a leaf should be computed from the full Bayes Net
VectorValues empty_values; VectorValues empty_values;
auto factorError = [&](const GaussianFactor::shared_ptr &factor) { auto factorError = [&](const GaussianFactor::shared_ptr &factor) {
if (!factor) return 0.0; // TODO(fan): does this make sense? if (!factor) return 0.0; // TODO(fan): does this make sense?
@ -383,6 +396,9 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
return discreteElimination(factors, frontalKeys); return discreteElimination(factors, frontalKeys);
} }
#ifdef HYBRID_TIMING
tictoc_reset_();
#endif
// Case 3: We are now in the hybrid land! // Case 3: We are now in the hybrid land!
return hybridElimination(factors, frontalKeys, continuousSeparator, return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparatorSet); discreteSeparatorSet);