nonrecursive version of optimizeWildfire for ISAM2 to avoid stack overflow problem

gtsam/nonlinear/ISAM2-impl.cpp

@@ -422,7 +422,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
 
   } else {
     // Optimize with wildfire
-    lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
+    lastBacksubVariableCount = optimizeWildfireNonRecursive(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
 
 #ifndef NDEBUG
     for(size_t j=0; j<delta.size(); ++j)
@@ -487,7 +487,7 @@ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThresho
   // Update variables
   size_t varsUpdated = 0;
   internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated);
-  optimizeWildfire(isam.root(), wildfireThreshold, replacedKeys, deltaNewton);
+  optimizeWildfireNonRecursive(isam.root(), wildfireThreshold, replacedKeys, deltaNewton);
 
 #if 0
   VectorValues expected = *allocateVectorValues(isam);

gtsam/nonlinear/ISAM2-inl.h

@@ -19,7 +19,7 @@
 
 #pragma once
 
-
+#include <stack>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/linear/JacobianFactor.h>
 
@@ -110,7 +110,82 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
     }
   }
 }
+
+template<class CLIQUE>
+bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
+    std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValues& delta, int& count) {
+  // if none of the variables in this clique (frontal and separator!) changed
+  // significantly, then by the running intersection property, none of the
+  // cliques in the children need to be processed
+
+  // Are any clique variables part of the tree that has been redone?
+  bool cliqueReplaced = replaced[(*clique)->frontals().front()];
+#ifndef NDEBUG
+  BOOST_FOREACH(Index frontal, (*clique)->frontals()) {
+    assert(cliqueReplaced == replaced[frontal]);
   }
+#endif
+
+  // If not redone, then has one of the separator variables changed significantly?
+  bool recalculate = cliqueReplaced;
+  if(!recalculate) {
+    BOOST_FOREACH(Index parent, (*clique)->parents()) {
+      if(changed[parent]) {
+        recalculate = true;
+        break;
+      }
+    }
+  }
+
+  // Solve clique if it was replaced, or if any parents were changed above the
+  // threshold or themselves replaced.
+  if(recalculate) {
+
+    // Temporary copy of the original values, to check how much they change
+    std::vector<Vector> originalValues((*clique)->nrFrontals());
+    GaussianConditional::const_iterator it;
+    for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
+      originalValues[it - (*clique)->beginFrontals()] = delta[*it];
+    }
+
+    // Back-substitute
+    (*clique)->solveInPlace(delta);
+    count += (*clique)->nrFrontals();
+
+    // Whether the values changed above a threshold, or always true if the
+    // clique was replaced.
+    bool valuesChanged = cliqueReplaced;
+    for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
+      if(!valuesChanged) {
+        const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
+        const SubVector& newValue(delta[*it]);
+        if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
+          valuesChanged = true;
+          break;
+        }
+      } else
+        break;
+    }
+
+    // If the values were above the threshold or this clique was replaced
+    if(valuesChanged) {
+      // Set changed flag for each frontal variable and leave the new values
+      BOOST_FOREACH(Index frontal, (*clique)->frontals()) {
+        changed[frontal] = true;
+      }
+    } else {
+      // Replace with the old values
+      for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
+        delta[*it] = originalValues[it - (*clique)->beginFrontals()];
+      }
+    }
+
+  }
+
+  return recalculate;
+}
+
+} // namespace internal
 
 /* ************************************************************************* */
 template<class CLIQUE>
@@ -123,6 +198,31 @@ int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, co
   return count;
 }
 
+/* ************************************************************************* */
+template<class CLIQUE>
+int optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValues& delta) {
+  std::vector<bool> changed(keys.size(), false);
+  int count = 0;
+
+  if (root) {
+    std::stack<boost::shared_ptr<CLIQUE> > travStack;
+    travStack.push(root);
+    boost::shared_ptr<CLIQUE> currentNode = root;
+    while (!travStack.empty()) {
+      currentNode = travStack.top();
+      travStack.pop();
+      bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count);
+      if (recalculate) {
+        BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children_) {
+          travStack.push(child);
+        }
+      }
+    }
+  }
+
+  return count;
+}
+
 /* ************************************************************************* */
 template<class CLIQUE>
 void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {

gtsam/nonlinear/ISAM2.h

@@ -640,6 +640,10 @@ template<class CLIQUE>
 int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
     double threshold, const std::vector<bool>& replaced, VectorValues& delta);
 
+template<class CLIQUE>
+int optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
+    double threshold, const std::vector<bool>& replaced, VectorValues& delta);
+
 /**
  * Optimize along the gradient direction, with a closed-form computation to
  * perform the line search.  The gradient is computed about \f$ \delta x=0 \f$.