Recalculate now removes top

gtsam/nonlinear/ISAM2-impl.h

@@ -187,9 +187,9 @@ struct GTSAM_EXPORT UpdateImpl {
 
   // Calculate nonlinear error
   void error(const NonlinearFactorGraph& nonlinearFactors,
-             const Values& estimate, boost::optional<double>* error) const {
+             const Values& estimate, boost::optional<double>* result) const {
     gttic(error);
-    error->reset(nonlinearFactors.error(estimate));
+    result->reset(nonlinearFactors.error(estimate));
   }
 
   // Mark linear update

gtsam/nonlinear/ISAM2.cpp

@@ -296,54 +296,64 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams,
 }
 
 /* ************************************************************************* */
-KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
+void ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
-                          const GaussianBayesNet& affectedBayesNet,
+                        const KeySet& relinKeys, ISAM2Result* result) {
-                          const KeySet& relinKeys, Cliques* orphans,
-                          ISAM2Result* result) {
   gttic(recalculate);
   UpdateImpl::LogRecalculateKeys(*result);
 
-  // FactorGraph<GaussianFactor> factors(affectedBayesNet);
+  if (!result->markedKeys.empty() || !result->observedKeys.empty()) {
-  // bug was here: we cannot reuse the original factors, because then the
+    // Remove top of Bayes tree and convert to a factor graph:
-  // cached factors get messed up [all the necessary data is actually
+    // (a) For each affected variable, remove the corresponding clique and all
-  // contained in the affectedBayesNet, including what was passed in from the
+    // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of
-  // boundaries, so this would be correct; however, in the process we also
+    // removed cliques.
-  // generate new cached_ entries that will be wrong (ie. they don't contain
+    GaussianBayesNet affectedBayesNet;
-  // what would be passed up at a certain point if batch elimination was done,
+    Cliques orphans;
-  // but that's what we need); we could choose not to update cached_ from
+    this->removeTop(
-  // here, but then the new information (and potentially different variable
+        KeyVector(result->markedKeys.begin(), result->markedKeys.end()),
-  // ordering) is not reflected in the cached_ values which again will be
+        &affectedBayesNet, &orphans);
-  // wrong] so instead we have to retrieve the original linearized factors AND
-  // add the cached factors from the boundary
 
-  // ordering provides all keys in conditionals, there cannot be others
+    // FactorGraph<GaussianFactor> factors(affectedBayesNet);
-  // because path to root included
+    // bug was here: we cannot reuse the original factors, because then the
-  gttic(affectedKeys);
+    // cached factors get messed up [all the necessary data is actually
-  FastList<Key> affectedKeys;
+    // contained in the affectedBayesNet, including what was passed in from the
-  for (const auto& conditional : affectedBayesNet)
+    // boundaries, so this would be correct; however, in the process we also
-    affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(),
+    // generate new cached_ entries that will be wrong (ie. they don't contain
-                        conditional->endFrontals());
+    // what would be passed up at a certain point if batch elimination was done,
-  gttoc(affectedKeys);
+    // but that's what we need); we could choose not to update cached_ from
+    // here, but then the new information (and potentially different variable
+    // ordering) is not reflected in the cached_ values which again will be
+    // wrong] so instead we have to retrieve the original linearized factors AND
+    // add the cached factors from the boundary
 
-  KeySet affectedKeysSet;  // Will return this result
+    // ordering provides all keys in conditionals, there cannot be others
+    // because path to root included
+    gttic(affectedKeys);
+    FastList<Key> affectedKeys;
+    for (const auto& conditional : affectedBayesNet)
+      affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(),
+                          conditional->endFrontals());
+    gttoc(affectedKeys);
 
-  static const double kBatchThreshold = 0.65;
+    KeySet affectedKeysSet;
-  if (affectedKeys.size() >= theta_.size() * kBatchThreshold) {
+    static const double kBatchThreshold = 0.65;
-    // Do a batch step - reorder and relinearize all variables
+    if (affectedKeys.size() >= theta_.size() * kBatchThreshold) {
-    recalculateBatch(updateParams, &affectedKeysSet, result);
+      // Do a batch step - reorder and relinearize all variables
-  } else {
+      recalculateBatch(updateParams, &affectedKeysSet, result);
-    recalculateIncremental(updateParams, relinKeys, affectedKeys,
+    } else {
-                           &affectedKeysSet, orphans, result);
+      recalculateIncremental(updateParams, relinKeys, affectedKeys,
+                             &affectedKeysSet, &orphans, result);
+    }
+
+    // Root clique variables for detailed results
+    if (result->detail && params_.enableDetailedResults) {
+      for (const auto& root : roots_)
+        for (Key var : *root->conditional())
+          result->detail->variableStatus[var].inRootClique = true;
+    }
+
+    // Update replaced keys mask (accumulates until back-substitution happens)
+    deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end());
   }
-
-  // Root clique variables for detailed results
-  if (result->detail && params_.enableDetailedResults) {
-    for (const auto& root : roots_)
-      for (Key var : *root->conditional())
-        result->detail->variableStatus[var].inRootClique = true;
-  }
-
-  return affectedKeysSet;
 }
 /* ************************************************************************* */
 void ISAM2::addVariables(const Values& newTheta,
@@ -471,22 +481,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
                               &variableIndex_);
 
   // 8. Redo top of Bayes tree
-  if (!result.markedKeys.empty() || !result.observedKeys.empty()) {
+  recalculate(updateParams, relinKeys, &result);
-    // Remove top of Bayes tree and convert to a factor graph:
-    // (a) For each affected variable, remove the corresponding clique and all
-    // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of
-    // removed cliques.
-    GaussianBayesNet affectedBayesNet;
-    Cliques orphans;
-    this->removeTop(
-        KeyVector(result.markedKeys.begin(), result.markedKeys.end()),
-        affectedBayesNet, orphans);
-
-    KeySet affectedKeysSet = recalculate(updateParams, affectedBayesNet,
-                                         relinKeys, &orphans, &result);
-    // Update replaced keys mask (accumulates until back-substitution happens)
-    deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end());
-  }
 
   // Update data structures to remove unused keys
   if (!result.unusedKeys.empty()) {

gtsam/nonlinear/ISAM2.h

@@ -296,11 +296,11 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
                               const FastList<Key>& affectedKeys,
                               KeySet* affectedKeysSet, Cliques* orphans,
                               ISAM2Result* result);
-
+  /**
-  KeySet recalculate(const ISAM2UpdateParams& updateParams,
+   * Remove marked top and either recalculate in batch or incrementally.
-                     const GaussianBayesNet& affectedBayesNet,
+   */
-                     const KeySet& relinKeys, Cliques* orphans,
+  void recalculate(const ISAM2UpdateParams& updateParams,
-                     ISAM2Result* result);
+                   const KeySet& relinKeys, ISAM2Result* result);
 
   /**
    * Add new variables to the ISAM2 system.