diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index b898c0520..ca3e27252 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -72,21 +72,17 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph, addConditionals(graph, hybridBayesNet_, ordering); // Eliminate. - HybridBayesNet::shared_ptr bayesNetFragment = - graph.eliminateSequential(ordering); + HybridBayesNet bayesNetFragment = *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->prune(*maxNrLeaves); - // Set the bayes net fragment to the pruned version - bayesNetFragment = std::make_shared(prunedBayesNetFragment); + bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves); } // Add the partial bayes net to the posterior bayes net. - hybridBayesNet_.add(*bayesNetFragment); + hybridBayesNet_.add(bayesNetFragment); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 267746ab6..66edf86d6 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/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. * diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 58decc695..88d8be0bc 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/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();