Add smoother printing
parent
555a2173a3
commit
ce031e8e81
|
@ -21,6 +21,7 @@
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <unordered_set>
|
#include <unordered_set>
|
||||||
|
|
||||||
|
// #define DEBUG_SMOOTHER
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -56,10 +57,16 @@ 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) {
|
||||||
|
std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() << std::endl;
|
||||||
|
std::cout << "newFactors size: " << graph.size() << std::endl;
|
||||||
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_);
|
||||||
|
// print size of graph, updatedGraph, hybridBayesNet_
|
||||||
|
std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl;
|
||||||
|
std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size() << std::endl;
|
||||||
|
std::cout << "total size: " << updatedGraph.size() + hybridBayesNet_.size() << std::endl;
|
||||||
|
|
||||||
Ordering ordering;
|
Ordering ordering;
|
||||||
// If no ordering provided, then we compute one
|
// If no ordering provided, then we compute one
|
||||||
|
@ -77,6 +84,19 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
|
||||||
// Eliminate.
|
// Eliminate.
|
||||||
HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering);
|
HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering);
|
||||||
|
|
||||||
|
#ifdef DEBUG_SMOOTHER
|
||||||
|
for (auto conditional: bayesNetFragment) {
|
||||||
|
auto e =std::dynamic_pointer_cast<HybridConditional::BaseConditional>(conditional);
|
||||||
|
GTSAM_PRINT(*e);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Print discrete keys in the bayesNetFragment:
|
||||||
|
std::cout << "Discrete keys in bayesNetFragment: ";
|
||||||
|
for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) {
|
||||||
|
std::cout << DefaultKeyFormatter(key) << " ";
|
||||||
|
}
|
||||||
|
|
||||||
/// Prune
|
/// Prune
|
||||||
if (maxNrLeaves) {
|
if (maxNrLeaves) {
|
||||||
// `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in
|
// `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in
|
||||||
|
@ -84,6 +104,20 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
|
||||||
bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, marginalThreshold_);
|
bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, marginalThreshold_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Print discrete keys in the bayesNetFragment:
|
||||||
|
std::cout << "\nAfter pruning: ";
|
||||||
|
for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) {
|
||||||
|
std::cout << DefaultKeyFormatter(key) << " ";
|
||||||
|
}
|
||||||
|
std::cout << std::endl << std::endl;
|
||||||
|
|
||||||
|
#ifdef DEBUG_SMOOTHER
|
||||||
|
for (auto conditional: bayesNetFragment) {
|
||||||
|
auto c =std::dynamic_pointer_cast<HybridConditional::BaseConditional>(conditional);
|
||||||
|
GTSAM_PRINT(*c);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Add the partial bayes net to the posterior bayes net.
|
// Add the partial bayes net to the posterior bayes net.
|
||||||
hybridBayesNet_.add(bayesNetFragment);
|
hybridBayesNet_.add(bayesNetFragment);
|
||||||
}
|
}
|
||||||
|
@ -117,6 +151,8 @@ 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
|
||||||
|
@ -129,6 +165,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
PrintKeySet(factorKeys);
|
||||||
|
|
||||||
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);
|
||||||
|
|
Loading…
Reference in New Issue