better docs and variable names

gtsam/nonlinear/ISAM2.cpp

@@ -556,10 +556,9 @@ ISAM2Result ISAM2::update(
 }
 
 /* ************************************************************************* */
-ISAM2Result ISAM2::update(
+ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
-    const NonlinearFactorGraph& newFactors,
     const Values& newTheta,
-    const ISAM2UpdateParams& up) {
+    const ISAM2UpdateParams& updateParams) {
   const bool debug = ISDEBUG("ISAM2 update");
   const bool verbose = ISDEBUG("ISAM2 update verbose");
 
@@ -577,7 +576,7 @@ ISAM2Result ISAM2::update(
   if (params_.enableDetailedResults)
     result.detail = ISAM2Result::DetailedResults();
   const bool relinearizeThisStep =
-      up.force_relinearize || (params_.enableRelinearization &&
+      updateParams.force_relinearize || (params_.enableRelinearization &&
                             update_count_ % params_.relinearizeSkip == 0);
 
   if (verbose) {
@@ -601,8 +600,8 @@ ISAM2Result ISAM2::update(
 
   // Remove the removed factors
   NonlinearFactorGraph removeFactors;
-  removeFactors.reserve(up.removeFactorIndices.size());
+  removeFactors.reserve(updateParams.removeFactorIndices.size());
-  for (const auto index : up.removeFactorIndices) {
+  for (const auto index : updateParams.removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
     if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
@@ -610,8 +609,8 @@ ISAM2Result ISAM2::update(
 
   // Remove removed factors from the variable index so we do not attempt to
   // relinearize them
-  variableIndex_.remove(up.removeFactorIndices.begin(),
+  variableIndex_.remove(updateParams.removeFactorIndices.begin(),
-                        up.removeFactorIndices.end(),
+                        updateParams.removeFactorIndices.end(),
                         removeFactors);
 
   // Compute unused keys and indices
@@ -666,16 +665,16 @@ ISAM2Result ISAM2::update(
         markedRemoveKeys.end());  // Add to the overall set of marked keys
   }
   // Also mark any provided extra re-eliminate keys
-  if (up.extraReelimKeys) {
+  if (updateParams.extraReelimKeys) {
-    for (Key key : *up.extraReelimKeys) {
+    for (Key key : *updateParams.extraReelimKeys) {
       markedKeys.insert(key);
     }
   }
   // Also, keys that were not observed in existing factors, but whose affected
   // keys have been extended now (e.g. smart factors)
-  if (up.newAffectedKeys) {
+  if (updateParams.newAffectedKeys) {
-    for (const auto &f2ks : up.newAffectedKeys.value()) {
+    for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) {
-      const auto factorIdx = f2ks.first;
+      const auto factorIdx = factorAddedKeys.first;
       const auto& affectedKeys = nonlinearFactors_.at(factorIdx)->keys();
       markedKeys.insert(affectedKeys.begin(),affectedKeys.end());
     }
@@ -718,8 +717,8 @@ ISAM2Result ISAM2::update(
     for (Key key : fixedVariables_) {
       relinKeys.erase(key);
     }
-    if (up.noRelinKeys) {
+    if (updateParams.noRelinKeys) {
-      for (Key key : *up.noRelinKeys) {
+      for (Key key : *updateParams.noRelinKeys) {
         relinKeys.erase(key);
       }
     }
@@ -798,10 +797,11 @@ ISAM2Result ISAM2::update(
     variableIndex_.augment(newFactors);
 
   // Augment it with existing factors which now affect to more variables:
-  if (up.newAffectedKeys) {
+  if (updateParams.newAffectedKeys) {
-    for (const auto &fk : *up.newAffectedKeys) {
+    for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) {
-      const auto factorIdx = fk.first;
+      const auto factorIdx = factorAddedKeys.first;
-      variableIndex_.augmentExistingFactor(factorIdx, fk.second);
+      variableIndex_.augmentExistingFactor(
+            factorIdx, factorAddedKeys.second);
     }
   }
   gttoc(augment_VI);
@@ -810,8 +810,9 @@ ISAM2Result ISAM2::update(
   // 8. Redo top of Bayes tree
   boost::shared_ptr<KeySet> replacedKeys;
   if (!markedKeys.empty() || !observedKeys.empty())
-    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys,
+    replacedKeys = recalculate(
-                               unusedIndices, up.constrainedKeys, &result);
+          markedKeys, relinKeys, observedKeys, unusedIndices,
+          updateParams.constrainedKeys, &result);
 
   // Update replaced keys mask (accumulates until back-substitution takes place)
   if (replacedKeys)

gtsam/nonlinear/ISAM2.h

@@ -158,18 +158,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
       bool force_relinearize = false);
 
   /**
-   * Alternative signature of update() with all additional parameters in one
+   * Add new factors, updating the solution and relinearizing as needed.
-   * structure. This form makes easier to keep future API/ABI compatibility if
-   * parameters change.
    *
-   * @param extraParams Additional parameters to control relinearization,
+   * Alternative signature of update() (see its documentation above), with all
+   * additional parameters in one structure. This form makes easier to keep
+   * future API/ABI compatibility if parameters change.
+   *
+   * @param newFactors The new factors to be added to the system
+   * @param newTheta Initialization points for new variables to be added to the
+   * system. You must include here all new variables occuring in newFactors
+   * (which were not already in the system).  There must not be any variables
+   * here that do not occur in newFactors, and additionally, variables that were
+   * already in the system must not be included here.
+   * @param updateParams 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);
+      const ISAM2UpdateParams& updateParams);
 
   /** Marginalize out variables listed in leafKeys.  These keys must be leaves
    * in the BayesTree.  Throws MarginalizeNonleafException if non-leaves are

gtsam/nonlinear/ISAM2UpdateParams.h

@@ -35,26 +35,33 @@ struct GTSAM_EXPORT ISAM2UpdateParams {
   /** 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
+  /** 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
+  /** 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
+  /** 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
+  /** Relinearize any variables whose delta magnitude is sufficiently large
    * (Params::relinearizeThreshold), regardless of the relinearization
    * interval (Params::relinearizeSkip). */
   bool force_relinearize{false};
 
   /** An optional set of new Keys that are now affected by factors,
    * indexed by factor indices (as returned by ISAM2::update()).
+   * Use when working with smart factors. For example:
+   *  - Timestamp `i`: ISAM2::update() called with a new smart factor depending
+   *    on Keys `X(0)` and `X(1)`. It returns that the factor index for the new
+   *    smart factor (inside ISAM2) is `13`.
+   *  - Timestamp `i+1`: The same smart factor has been augmented to now also
+   *    depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include
+   *    its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`.
    */
   boost::optional<FastMap<FactorIndex,KeySet>> newAffectedKeys{boost::none};