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

Guard all printing

release/4.3a0
Frank Dellaert 2025-01-30 10:11:20 -05:00
parent efea6b8cb9
commit a1467c5e84
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/hybrid/HybridSmoother.cpp
View File

@ -57,19 +57,23 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
void HybridSmoother::update(const HybridGaussianFactorGraph &graph, void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
std::optional<size_t> maxNrLeaves, std::optional<size_t> maxNrLeaves,
const std::optional<Ordering> given_ordering) { const std::optional<Ordering> given_ordering) {
#ifdef DEBUG_SMOOTHER
std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size()
<< std::endl; << std::endl;
std::cout << "newFactors size: " << graph.size() << std::endl; std::cout << "newFactors size: " << graph.size() << std::endl;
#endif
HybridGaussianFactorGraph updatedGraph; HybridGaussianFactorGraph updatedGraph;
// Add the necessary conditionals from the previous timestep(s). // Add the necessary conditionals from the previous timestep(s).
std::tie(updatedGraph, hybridBayesNet_) = std::tie(updatedGraph, hybridBayesNet_) =
addConditionals(graph, hybridBayesNet_); addConditionals(graph, hybridBayesNet_);
#ifdef DEBUG_SMOOTHER
// print size of graph, updatedGraph, hybridBayesNet_ // print size of graph, updatedGraph, hybridBayesNet_
std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl; std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl;
std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size() std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size()
<< std::endl; << std::endl;
std::cout << "total size: " << updatedGraph.size() + hybridBayesNet_.size() std::cout << "total size: " << updatedGraph.size() + hybridBayesNet_.size()
<< std::endl; << std::endl;
#endif
Ordering ordering; Ordering ordering;
// If no ordering provided, then we compute one // If no ordering provided, then we compute one
@ -87,7 +91,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
// Eliminate. // Eliminate.
HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering); HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering);
#ifdef DEBUG_SMOOTHER #ifdef DEBUG_SMOOTHER_DETAIL
for (auto conditional : bayesNetFragment) { for (auto conditional : bayesNetFragment) {
auto e = std::dynamic_pointer_cast<HybridConditional::BaseConditional>( auto e = std::dynamic_pointer_cast<HybridConditional::BaseConditional>(
conditional); conditional);
@ -95,11 +99,13 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
} }
#endif #endif
#ifdef DEBUG_SMOOTHER
// Print discrete keys in the bayesNetFragment: // Print discrete keys in the bayesNetFragment:
std::cout << "Discrete keys in bayesNetFragment: "; std::cout << "Discrete keys in bayesNetFragment: ";
for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) { for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
#endif
/// Prune /// Prune
if (maxNrLeaves) { if (maxNrLeaves) {
@ -111,14 +117,16 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
fixedValues_.insert(newlyFixedValues); fixedValues_.insert(newlyFixedValues);
} }
#ifdef DEBUG_SMOOTHER
// Print discrete keys in the bayesNetFragment: // Print discrete keys in the bayesNetFragment:
std::cout << "\nAfter pruning: "; std::cout << "\nAfter pruning: ";
for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) { for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
std::cout << std::endl << std::endl; std::cout << std::endl << std::endl;
#endif
#ifdef DEBUG_SMOOTHER #ifdef DEBUG_SMOOTHER_DETAIL
for (auto conditional : bayesNetFragment) { for (auto conditional : bayesNetFragment) {
auto c = std::dynamic_pointer_cast<HybridConditional::BaseConditional>( auto c = std::dynamic_pointer_cast<HybridConditional::BaseConditional>(
conditional); conditional);
@ -159,8 +167,6 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
auto conditional = hybridBayesNet.at(i); auto conditional = hybridBayesNet.at(i);
for (auto &key : conditional->frontals()) { for (auto &key : conditional->frontals()) {
// GTSAM_PRINT(*std::dynamic_pointer_cast<HybridConditional::BaseConditional>(conditional));
// GTSAM_PRINT(*conditional);
if (std::find(factorKeys.begin(), factorKeys.end(), key) != if (std::find(factorKeys.begin(), factorKeys.end(), key) !=
factorKeys.end()) { factorKeys.end()) {
// Add the conditional parents to factorKeys // Add the conditional parents to factorKeys
@ -173,7 +179,9 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
} }
} }
} }
#ifdef DEBUG_SMOOTHER
PrintKeySet(factorKeys); PrintKeySet(factorKeys);
#endif
for (size_t i = 0; i < hybridBayesNet.size(); i++) { for (size_t i = 0; i < hybridBayesNet.size(); i++) {
auto conditional = hybridBayesNet.at(i); auto conditional = hybridBayesNet.at(i);