From 57659261227a07806ab766e88638dd7b0fbd667b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 16 Sep 2022 10:47:01 -0400
Subject: [PATCH] optional ordering argument for HybridNonlinearISAM::update

---
 gtsam/hybrid/HybridNonlinearISAM.cpp | 9 ++++++---
 gtsam/hybrid/HybridNonlinearISAM.h   | 4 +++-
 2 files changed, 9 insertions(+), 4 deletions(-)

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<size_t>& maxNrLeaves,
+                                 const boost::optional<Ordering>& 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<size_t>& maxNrLeaves = boost::none,
+              const boost::optional<Ordering>& ordering = boost::none);
 
   /** Relinearization and reordering of variables */
   void reorder_relinearize();