made gatherInvolvedKeys more explicit

gtsam/nonlinear/ISAM2-impl.h

@@ -129,7 +129,6 @@ struct GTSAM_EXPORT UpdateImpl {
       isam2.print("ISAM2: ");
     }
 
-    // Add the new factor indices to the result struct
     if (debug || verbose) {
       newFactors.print("The new factors are: ");
     }
@@ -189,12 +188,13 @@ struct GTSAM_EXPORT UpdateImpl {
   // Mark linear update
   void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
                           const NonlinearFactorGraph& nonlinearFactors,
-                          KeySet* markedKeys, ISAM2Result* result) const {
+                          const KeySet& keysWithRemovedFactors,
+                          KeySet* markedKeys) const {
     gttic(gatherInvolvedKeys);
     *markedKeys = newFactors.keys();  // Get keys from new factors
     // Also mark keys involved in removed factors
-    markedKeys->insert(result->keysWithRemovedFactors.begin(),
+    markedKeys->insert(keysWithRemovedFactors.begin(),
-                       result->keysWithRemovedFactors.end());
+                       keysWithRemovedFactors.end());
 
     // Also mark any provided extra re-eliminate keys
     if (updateParams_.extraReelimKeys) {
@@ -212,15 +212,19 @@ struct GTSAM_EXPORT UpdateImpl {
         markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
       }
     }
+  }
 
+  // Update detail, unused, and observed keys from markedKeys
+  void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
+    gttic(updateKeys);
     // Observed keys for detailed results
     if (params_.enableDetailedResults) {
-      for (Key key : *markedKeys) {
+      for (Key key : markedKeys) {
         result->detail->variableStatus[key].isObserved = true;
       }
     }
 
-    for (Key index : *markedKeys) {
+    for (Key index : markedKeys) {
       // Only add if not unused
       if (result->unusedKeys.find(index) == result->unusedKeys.end())
         // Make a copy of these, as we'll soon add to them
@@ -285,13 +289,14 @@ struct GTSAM_EXPORT UpdateImpl {
 
   /**
    * Find the set of variables to be relinearized according to
-   * relinearizeThreshold. This check is performed recursively, starting at the
+   * relinearizeThreshold. This check is performed recursively, starting at
-   * top of the tree. Once a variable in the tree does not need to be
+   * the top of the tree. Once a variable in the tree does not need to be
    * relinearized, no further checks in that branch are performed. This is an
-   * approximation of the Full version, designed to save time at the expense of
+   * approximation of the Full version, designed to save time at the expense
-   * accuracy.
+   * of accuracy.
    * @param delta The linear delta to check against the threshold
-   * @param keyFormatter Formatter for printing nonlinear keys during debugging
+   * @param keyFormatter Formatter for printing nonlinear keys during
+   * debugging
    * @return The set of variable indices in delta whose magnitude is greater
    * than or equal to relinearizeThreshold
    */
@@ -313,10 +318,12 @@ struct GTSAM_EXPORT UpdateImpl {
 
   /**
    * Find the set of variables to be relinearized according to
-   * relinearizeThreshold. Any variables in the VectorValues delta whose vector
+   * relinearizeThreshold. Any variables in the VectorValues delta whose
-   * magnitude is greater than or equal to relinearizeThreshold are returned.
+   * vector magnitude is greater than or equal to relinearizeThreshold are
+   * returned.
    * @param delta The linear delta to check against the threshold
-   * @param keyFormatter Formatter for printing nonlinear keys during debugging
+   * @param keyFormatter Formatter for printing nonlinear keys during
+   * debugging
    * @return The set of variable indices in delta whose magnitude is greater
    * than or equal to relinearizeThreshold
    */
@@ -504,8 +511,8 @@ struct GTSAM_EXPORT UpdateImpl {
     return indices;
   }
 
-  // find intermediate (linearized) factors from cache that are passed into the
+  // find intermediate (linearized) factors from cache that are passed into
-  // affected area
+  // the affected area
   static GaussianFactorGraph GetCachedBoundaryFactors(
       const ISAM2::Cliques& orphans) {
     GaussianFactorGraph cachedBoundary;

gtsam/nonlinear/ISAM2.cpp

@@ -438,8 +438,9 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
   gttoc(evaluate_error_before);
 
   // 3. Mark linear update
-  update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result.markedKeys,
+  update.gatherInvolvedKeys(newFactors, nonlinearFactors_,
-                            &result);
+                            result.keysWithRemovedFactors, &result.markedKeys);
+  update.updateKeys(result.markedKeys, &result);
 
   // Check relinearization if we're at the nth step, or we are using a looser
   // loop relinerization threshold.