Alternative ISAM2::update() signature, easier to extend.

The old update() method still exists and calls the new one.
The idea is that by using a struct, it is easier (i.e. API/ABI
compatibility) to add the new parameters that we will need to add next.

gtsam/nonlinear/ISAM2.cpp

@@ -544,6 +544,22 @@ ISAM2Result ISAM2::update(
     const boost::optional<FastList<Key> >& noRelinKeys,
     const boost::optional<FastList<Key> >& extraReelimKeys,
     bool force_relinearize) {
+
+  ISAM2UpdateParams params;
+  params.constrainedKeys = constrainedKeys;
+  params.extraReelimKeys = extraReelimKeys;
+  params.force_relinearize = force_relinearize;
+  params.noRelinKeys = noRelinKeys;
+  params.removeFactorIndices = removeFactorIndices;
+
+  return update(newFactors, newTheta, params);
+}
+
+/* ************************************************************************* */
+ISAM2Result ISAM2::update(
+    const NonlinearFactorGraph& newFactors,
+    const Values& newTheta,
+    const ISAM2UpdateParams& up) {
   const bool debug = ISDEBUG("ISAM2 update");
   const bool verbose = ISDEBUG("ISAM2 update verbose");
 
@@ -561,7 +577,7 @@ ISAM2Result ISAM2::update(
   if (params_.enableDetailedResults)
     result.detail = ISAM2Result::DetailedResults();
   const bool relinearizeThisStep =
-      force_relinearize || (params_.enableRelinearization &&
+      up.force_relinearize || (params_.enableRelinearization &&
                             update_count_ % params_.relinearizeSkip == 0);
 
   if (verbose) {
@@ -585,8 +601,8 @@ ISAM2Result ISAM2::update(
 
   // Remove the removed factors
   NonlinearFactorGraph removeFactors;
-  removeFactors.reserve(removeFactorIndices.size());
+  removeFactors.reserve(up.removeFactorIndices.size());
-  for (const auto index : removeFactorIndices) {
+  for (const auto index : up.removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
     if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
@@ -594,7 +610,8 @@ ISAM2Result ISAM2::update(
 
   // Remove removed factors from the variable index so we do not attempt to
   // relinearize them
-  variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(),
+  variableIndex_.remove(up.removeFactorIndices.begin(),
+                        up.removeFactorIndices.end(),
                         removeFactors);
 
   // Compute unused keys and indices
@@ -649,8 +666,8 @@ ISAM2Result ISAM2::update(
         markedRemoveKeys.end());  // Add to the overall set of marked keys
   }
   // Also mark any provided extra re-eliminate keys
-  if (extraReelimKeys) {
+  if (up.extraReelimKeys) {
-    for (Key key : *extraReelimKeys) {
+    for (Key key : *up.extraReelimKeys) {
       markedKeys.insert(key);
     }
   }
@@ -695,8 +712,8 @@ ISAM2Result ISAM2::update(
     for (Key key : fixedVariables_) {
       relinKeys.erase(key);
     }
-    if (noRelinKeys) {
+    if (up.noRelinKeys) {
-      for (Key key : *noRelinKeys) {
+      for (Key key : *up.noRelinKeys) {
         relinKeys.erase(key);
       }
     }
@@ -780,7 +797,7 @@ ISAM2Result ISAM2::update(
   boost::shared_ptr<KeySet> replacedKeys;
   if (!markedKeys.empty() || !observedKeys.empty())
     replacedKeys = recalculate(markedKeys, relinKeys, observedKeys,
-                               unusedIndices, constrainedKeys, &result);
+                               unusedIndices, up.constrainedKeys, &result);
 
   // Update replaced keys mask (accumulates until back-substitution takes place)
   if (replacedKeys)

gtsam/nonlinear/ISAM2.h

@@ -23,6 +23,7 @@
 #include <gtsam/nonlinear/ISAM2Params.h>
 #include <gtsam/nonlinear/ISAM2Result.h>
 #include <gtsam/nonlinear/ISAM2Clique.h>
+#include <gtsam/nonlinear/ISAM2UpdateParams.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/linear/GaussianBayesTree.h>
 
@@ -156,6 +157,20 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
       const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
       bool force_relinearize = false);
 
+  /**
+   * Alternative signature of update() with all additional parameters in one
+   * structure. This form makes easier to keep future API/ABI compatibility if
+   * parameters change.
+   *
+   * @param extraParams Additional parameters to control relinearization,
+   * constrained keys, etc.
+   * @return An ISAM2Result struct containing information about the update
+   * @note No default parameters to avoid ambiguous call errors.
+   */
+  virtual ISAM2Result update(
+      const NonlinearFactorGraph& newFactors, const Values& newTheta,
+      const ISAM2UpdateParams& up);
+
   /** Marginalize out variables listed in leafKeys.  These keys must be leaves
    * in the BayesTree.  Throws MarginalizeNonleafException if non-leaves are
    * requested to be marginalized.  Marginalization leaves a linear

gtsam/nonlinear/ISAM2UpdateParams.h

@@ -0,0 +1,57 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    ISAM2UpdateParams.h
+ * @brief   Class that stores extra params for ISAM2::update()
+ * @author  Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <boost/optional.hpp>
+#include <gtsam/base/FastList.h>
+#include <gtsam/dllexport.h>             // GTSAM_EXPORT
+#include <gtsam/inference/Key.h>         // Key, KeySet
+#include <gtsam/nonlinear/ISAM2Result.h> //FactorIndices
+
+namespace gtsam {
+
+/**
+ * @addtogroup ISAM2
+ * This struct is used by ISAM2::update() to pass additional parameters to
+ * give the user a fine-grained control on how factors and relinearized, etc.
+ */
+struct GTSAM_EXPORT ISAM2UpdateParams {
+  ISAM2UpdateParams() = default;
+
+  /** Indices of factors to remove from system (default: empty) */
+  FactorIndices removeFactorIndices;
+
+  /* An optional map of keys to group labels, such that a variable can be
+   * constrained to a particular grouping in the BayesTree */
+  boost::optional<FastMap<Key, int>> constrainedKeys{boost::none};
+
+  /* An optional set of nonlinear keys that iSAM2 will hold at a constant
+   * linearization point, regardless of the size of the linear delta */
+  boost::optional<FastList<Key>> noRelinKeys{boost::none};
+
+  /* An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless
+   * of the size of the linear delta. This allows the provided keys to be
+   * reordered. */
+  boost::optional<FastList<Key>> extraReelimKeys{boost::none};
+
+  /* Relinearize any variables whose delta magnitude is sufficiently large
+   * (Params::relinearizeThreshold), regardless of the relinearization
+   * interval (Params::relinearizeSkip). */
+  bool force_relinearize{false};
+};
+
+} // namespace gtsam