diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 36cda4e80..d05e081dd 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -33,7 +33,9 @@ void HybridNonlinearISAM::saveGraph(const string& s, /* ************************************************************************* */ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, - const Values& initialValues) { + const Values& initialValues, + const boost::optional& maxNrLeaves, + const boost::optional& ordering) { if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { @@ -51,7 +53,8 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, newFactors.linearize(linPoint_); // Update ISAM - isam_.update(*linearizedNewFactors, boost::none, eliminationFunction_); + isam_.update(*linearizedNewFactors, maxNrLeaves, ordering, + eliminationFunction_); } } @@ -66,7 +69,7 @@ void HybridNonlinearISAM::reorder_relinearize() { // Just recreate the whole BayesTree // TODO: allow for constrained ordering here // TODO: decouple relinearization and reordering to avoid - isam_.update(*factors_.linearize(newLinPoint), boost::none, + isam_.update(*factors_.linearize(newLinPoint), boost::none, boost::none, eliminationFunction_); // Update linearization point diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index b1998fb30..47aa81c55 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -118,7 +118,9 @@ class GTSAM_EXPORT HybridNonlinearISAM { /** Add new factors along with their initial linearization points */ void update(const HybridNonlinearFactorGraph& newFactors, - const Values& initialValues); + const Values& initialValues, + const boost::optional& maxNrLeaves = boost::none, + const boost::optional& ordering = boost::none); /** Relinearization and reordering of variables */ void reorder_relinearize();