Fix smoother

gtsam/hybrid/HybridSmoother.cpp

@@ -72,21 +72,17 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph,
       addConditionals(graph, hybridBayesNet_, ordering);
 
   // Eliminate.
-  HybridBayesNet::shared_ptr bayesNetFragment =
+  HybridBayesNet bayesNetFragment = *graph.eliminateSequential(ordering);
-      graph.eliminateSequential(ordering);
 
   /// Prune
   if (maxNrLeaves) {
     // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in
     // all the conditionals with the same keys in bayesNetFragment.
-    HybridBayesNet prunedBayesNetFragment =
+    bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves);
-        bayesNetFragment->prune(*maxNrLeaves);
-    // Set the bayes net fragment to the pruned version
-    bayesNetFragment = std::make_shared<HybridBayesNet>(prunedBayesNetFragment);
   }
 
   // Add the partial bayes net to the posterior bayes net.
-  hybridBayesNet_.add(*bayesNetFragment);
+  hybridBayesNet_.add(bayesNetFragment);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/HybridSmoother.h

@@ -39,7 +39,7 @@ class GTSAM_EXPORT HybridSmoother {
    * discrete factor on all discrete keys, plus all discrete factors in the
    * original graph.
    *
-   * \note If maxComponents is given, we look at the discrete factor resulting
+   * \note If maxNrLeaves is given, we look at the discrete factor resulting
    * from this elimination, and prune it and the Gaussian components
    * corresponding to the pruned choices.
    *

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -109,6 +109,7 @@ TEST(HybridEstimation, IncrementalSmoother) {
 
   HybridGaussianFactorGraph linearized;
 
+  constexpr size_t maxNrLeaves = 3;
   for (size_t k = 1; k < K; k++) {
     // Motion Model
     graph.push_back(switching.nonlinearFactorGraph.at(k));
@@ -120,8 +121,12 @@ TEST(HybridEstimation, IncrementalSmoother) {
     linearized = *graph.linearize(initial);
     Ordering ordering = smoother.getOrdering(linearized);
 
-    smoother.update(linearized, 3, ordering);
+    smoother.update(linearized, maxNrLeaves, ordering);
     graph.resize(0);
+
+    // Uncomment to print out pruned discrete marginal:
+    // smoother.hybridBayesNet().at(0)->asDiscrete()->dot("smoother_" +
+    //                                                    std::to_string(k));
   }
 
   HybridValues delta = smoother.hybridBayesNet().optimize();