optional maxNrLeaves for HybridGaussianISAM

gtsam/hybrid/HybridGaussianISAM.cpp

@@ -42,6 +42,7 @@ HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
 void HybridGaussianISAM::updateInternal(
     const HybridGaussianFactorGraph& newFactors,
     HybridBayesTree::Cliques* orphans,
+    const boost::optional<size_t>& maxNrLeaves,
     const boost::optional<Ordering>& ordering,
     const HybridBayesTree::Eliminate& function) {
   // Remove the contaminated part of the Bayes tree
@@ -92,6 +93,10 @@ void HybridGaussianISAM::updateInternal(
   HybridBayesTree::shared_ptr bayesTree =
       factors.eliminateMultifrontal(elimination_ordering, function, index);
 
+  if (maxNrLeaves) {
+    bayesTree->prune(*maxNrLeaves);
+  }
+
   // Re-add into Bayes tree data structures
   this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
                       bayesTree->roots().end());
@@ -100,10 +105,11 @@ void HybridGaussianISAM::updateInternal(
 
 /* ************************************************************************* */
 void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
+                                const boost::optional<size_t>& maxNrLeaves,
                                 const boost::optional<Ordering>& ordering,
                                 const HybridBayesTree::Eliminate& function) {
   Cliques orphans;
-  this->updateInternal(newFactors, &orphans, ordering, function);
+  this->updateInternal(newFactors, &orphans, maxNrLeaves, ordering, function);
 }
 
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianISAM.h

@@ -48,6 +48,7 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
   void updateInternal(
       const HybridGaussianFactorGraph& newFactors,
       HybridBayesTree::Cliques* orphans,
+      const boost::optional<size_t>& maxNrLeaves = boost::none,
       const boost::optional<Ordering>& ordering = boost::none,
       const HybridBayesTree::Eliminate& function =
           HybridBayesTree::EliminationTraitsType::DefaultEliminate);
@@ -57,19 +58,15 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
    * @brief Perform update step with new factors.
    *
    * @param newFactors Factor graph of new factors to add and eliminate.
+   * @param maxNrLeaves The maximum number of leaves to keep after pruning.
+   * @param ordering Custom elimination ordering.
    * @param function Elimination function.
    */
   void update(const HybridGaussianFactorGraph& newFactors,
+              const boost::optional<size_t>& maxNrLeaves = boost::none,
               const boost::optional<Ordering>& ordering = boost::none,
               const HybridBayesTree::Eliminate& function =
                   HybridBayesTree::EliminationTraitsType::DefaultEliminate);
-
-  /**
-   * @brief Prune the underlying Bayes tree.
-   *
-   * @param maxNumberLeaves The max number of leaf nodes to keep.
-   */
-  void prune(const size_t maxNumberLeaves);
 };
 
 /// traits