Add smoother printing

gtsam/hybrid/HybridSmoother.cpp

@@ -21,6 +21,7 @@
 #include <algorithm>
 #include <unordered_set>
 
+// #define DEBUG_SMOOTHER
 namespace gtsam {
 
 /* ************************************************************************* */
@@ -56,10 +57,16 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
 void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
                             std::optional<size_t> maxNrLeaves,
                             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;
   // Add the necessary conditionals from the previous timestep(s).
   std::tie(updatedGraph, 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;
   // If no ordering provided, then we compute one
@@ -77,6 +84,19 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
   // Eliminate.
   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
   if (maxNrLeaves) {
     // `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_);
   }
 
+  // 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.
   hybridBayesNet_.add(bayesNetFragment);
 }
@@ -117,6 +151,8 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
       auto conditional = hybridBayesNet.at(i);
 
       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) !=
             factorKeys.end()) {
           // 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++) {
       auto conditional = hybridBayesNet.at(i);