Only do ISAM2 back-substitution when needed instead of during every update

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -46,8 +46,8 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
    * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
    * @param keyFormatter Formatter for printing nonlinear keys during debugging
    */
-  static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering,
+  static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
-      typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+      Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol
@@ -121,12 +121,15 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
   static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
       const ReorderingMode& reorderingMode);
 
+  static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
+
 };
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
-    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
+    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
+    Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
   theta.insert(newTheta);
@@ -153,6 +156,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
     assert(ordering.size() == delta.size());
   }
   assert(ordering.nVars() >= nodes.size());
+  replacedKeys.resize(ordering.nVars(), false);
   nodes.resize(ordering.nVars());
 }
 
@@ -229,7 +233,8 @@ void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
   invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
 #endif
 
-  assert(values.size() == ordering.size());
+  assert(values.size() == ordering.nVars());
+  assert(delta.size() == ordering.nVars());
   Values::iterator key_value;
   Ordering::const_iterator key_index;
   for(key_value = values.begin(), key_index = ordering.begin();
@@ -354,4 +359,43 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
   return result;
 }
 
+/* ************************************************************************* */
+template<class CONDITIONAL, class GRAPH>
+size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
+
+  size_t lastBacksubVariableCount;
+
+  if (wildfireThreshold <= 0.0) {
+    // Threshold is zero or less, so do a full recalculation
+    // Collect dimensions and allocate new VectorValues
+    vector<size_t> dims(delta.size());
+    for(size_t j=0; j<delta.size(); ++j)
+      dims[j] = delta->dim(j);
+    VectorValues newDelta(dims);
+
+    // Optimize full solution delta
+    optimize2(root, newDelta);
+
+    // Copy solution into delta
+    delta.permutation() = Permutation::Identity(delta.size());
+    delta.container() = newDelta;
+
+    lastBacksubVariableCount = delta.size();
+
+  } else {
+    // Optimize with wildfire
+    lastBacksubVariableCount = optimize2(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
+
+#ifndef NDEBUG
+    for(size_t j=0; j<delta.container().size(); ++j)
+      assert(delta.container()[j].unaryExpr(&isfinite<double>).all());
+#endif
+  }
+
+  // Clear replacedKeys
+  replacedKeys.assign(replacedKeys.size(), false);
+
+  return lastBacksubVariableCount;
+}
+
 }

gtsam/nonlinear/ISAM2-inl.h

@@ -41,7 +41,7 @@ static const double batchThreshold = 0.65;
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
-    delta_(Permutation(), deltaUnpermuted_), params_(params) {
+    delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@@ -50,7 +50,7 @@ ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 ISAM2<CONDITIONAL, GRAPH>::ISAM2():
-    delta_(Permutation(), deltaUnpermuted_) {
+    delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@@ -413,13 +413,21 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
   lastBacksubVariableCount = 0;
   lastNnzTop = 0;
   ISAM2Result result;
+  const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0);
 
   if(verbose) {
     cout << "ISAM2::update\n";
     this->print("ISAM2: ");
   }
 
-  tic(0,"push_back factors");
+  // Update delta if we need it to check relinearization later
+  if(relinearizeThisStep) {
+    tic(0, "updateDelta");
+    updateDelta(disableReordering);
+    toc(0, "updateDelta");
+  }
+
+  tic(1,"push_back factors");
   // Add the new factor indices to the result struct
   result.newFactorsIndices.resize(newFactors.size());
   for(size_t i=0; i<newFactors.size(); ++i)
@@ -438,19 +446,19 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
 
   // Remove removed factors from the variable index so we do not attempt to relinearize them
   variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
-  toc(0,"push_back factors");
+  toc(1,"push_back factors");
 
-  tic(1,"add new variables");
+  tic(2,"add new variables");
   // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
-  Impl::AddVariables(newTheta, theta_, delta_, ordering_, Base::nodes_);
+  Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_);
-  toc(1,"add new variables");
+  toc(2,"add new variables");
 
-  tic(2,"evaluate error before");
+  tic(3,"evaluate error before");
   if(params_.evaluateNonlinearError)
     result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate()));
-  toc(2,"evaluate error before");
+  toc(3,"evaluate error before");
 
-  tic(3,"gather involved keys");
+  tic(4,"gather involved keys");
   // 3. Mark linear update
   FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
   // Also mark keys involved in removed factors
@@ -462,11 +470,11 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
   // is a vector of size_t, so the constructor unintentionally resolves to
   // vector(size_t count, Index value) instead of the iterator constructor.
   FastVector<Index> newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end());             // Make a copy of these, as we'll soon add to them
-  toc(3,"gather involved keys");
+  toc(4,"gather involved keys");
 
   // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
-  if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps
+  if (relinearizeThisStep) {
-    tic(4,"gather relinearize keys");
+    tic(5,"gather relinearize keys");
     vector<bool> markedRelinMask(ordering_.nVars(), false);
     // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
     FastSet<Index> relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
@@ -475,19 +483,19 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
     // Add the variables being relinearized to the marked keys
     BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
     markedKeys.insert(relinKeys.begin(), relinKeys.end());
-    toc(4,"gather relinearize keys");
+    toc(5,"gather relinearize keys");
 
-    tic(5,"fluid find_all");
+    tic(6,"fluid find_all");
     // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
     if (!relinKeys.empty() && this->root())
       Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator
-    toc(5,"fluid find_all");
+    toc(6,"fluid find_all");
 
-    tic(6,"expmap");
+    tic(7,"expmap");
     // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
     if (!relinKeys.empty())
       Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_);
-    toc(6,"expmap");
+    toc(7,"expmap");
 
     result.variablesRelinearized = markedKeys.size();
 
@@ -501,7 +509,7 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
 #endif
   }
 
-  tic(7,"linearize new");
+  tic(8,"linearize new");
   tic(1,"linearize");
   // 7. Linearize new factors
   FactorGraph<GaussianFactor>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
@@ -511,9 +519,9 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
   // Augment the variable index with the new factors
   variableIndex_.augment(*linearFactors);
   toc(2,"augment VI");
-  toc(7,"linearize new");
+  toc(8,"linearize new");
 
-  tic(8,"recalculate");
+  tic(9,"recalculate");
   // 8. Redo top of Bayes tree
   // Convert constrained symbols to indices
   boost::optional<FastSet<Index> > constrainedIndices;
@@ -526,49 +534,17 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
   boost::shared_ptr<FastSet<Index> > replacedKeys;
   if(!markedKeys.empty() || !newKeys.empty())
     replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result);
-  toc(8,"recalculate");
 
-  tic(9,"solve");
+  // Update replaced keys mask (accumulates until back-substitution takes place)
+  if(replacedKeys) {
+    BOOST_FOREACH(const Index var, *replacedKeys) {
+      deltaReplacedMask_[var] = true; } }
+  toc(9,"recalculate");
+
+  //tic(9,"solve");
   // 9. Solve
-  if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
+  if(debug) delta_.print("delta_: ");
-  // See note in gtsam/base/boost_variant_with_workaround.h
+  //toc(9,"solve");
-    const ISAM2GaussNewtonParams& gaussNewtonParams =
-        boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
-    if (gaussNewtonParams.wildfireThreshold <= 0.0 || disableReordering) {
-      VectorValues newDelta(theta_.dims(ordering_));
-      optimize2(this->root(), newDelta);
-      if(debug) newDelta.print("newDelta: ");
-      assert(newDelta.size() == delta_.size());
-      delta_.permutation() = Permutation::Identity(delta_.size());
-      delta_.container() = newDelta;
-      lastBacksubVariableCount = theta_.size();
-    } else {
-      vector<bool> replacedKeysMask(variableIndex_.size(), false);
-      if(replacedKeys) {
-        BOOST_FOREACH(const Index var, *replacedKeys) {
-          replacedKeysMask[var] = true; } }
-      lastBacksubVariableCount = optimize2(this->root(), gaussNewtonParams.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_
-
-#ifndef NDEBUG
-      for(size_t j=0; j<delta_.container().size(); ++j)
-        assert(delta_.container()[j].unaryExpr(&isfinite<double>).all());
-#endif
-    }
-  } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
-  // See note in gtsam/base/boost_variant_with_workaround.h
-    const ISAM2DoglegParams& doglegParams =
-        boost::get<ISAM2DoglegParams>(params_.optimizationParams);
-    // Do one Dogleg iteration
-    tic(1, "Dogleg Iterate");
-    DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate(
-        *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose);
-    toc(1, "Dogleg Iterate");
-    // Update Delta and linear step
-    doglegDelta_ = doglegResult.Delta;
-    delta_.permutation() = Permutation::Identity(delta_.size());  // Dogleg solves for the full delta so there is no permutation
-    delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
-  }
-  toc(9,"solve");
 
   tic(10,"evaluate error after");
   if(params_.evaluateNonlinearError)
@@ -576,10 +552,48 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
   toc(10,"evaluate error after");
 
   result.cliques = this->nodes().size();
+  deltaUptodate_ = false;
 
   return result;
 }
 
+
+/* ************************************************************************* */
+template<class CONDITIONAL, class GRAPH>
+void ISAM2<CONDITIONAL, GRAPH>::updateDelta(bool forceFullSolve) const {
+
+  if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
+    // If using Gauss-Newton, update with wildfireThreshold
+    const ISAM2GaussNewtonParams& gaussNewtonParams =
+        boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
+    const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold;
+    tic(0, "Wildfire update");
+    lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold);
+    toc(0, "Wildfire update");
+
+  } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
+    // If using Dogleg, do a Dogleg step
+    const ISAM2DoglegParams& doglegParams =
+        boost::get<ISAM2DoglegParams>(params_.optimizationParams);
+
+    // Do one Dogleg iteration
+    tic(1, "Dogleg Iterate");
+    DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate(
+        *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose);
+    toc(1, "Dogleg Iterate");
+
+    // Update Delta and linear step
+    doglegDelta_ = doglegResult.Delta;
+    delta_.permutation() = Permutation::Identity(delta_.size());  // Dogleg solves for the full delta so there is no permutation
+    delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
+
+    // Clear replaced mask
+    deltaReplacedMask_.assign(deltaReplacedMask_.size(), false);
+  }
+
+  deltaUptodate_ = true;
+}
+
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
@@ -587,7 +601,7 @@ Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
   // handles Permuted<VectorValues>
 	Values ret(theta_);
   vector<bool> mask(ordering_.nVars(), true);
-  Impl::ExpmapMasked(ret, delta_, ordering_, mask);
+  Impl::ExpmapMasked(ret, getDelta(), ordering_, mask);
   return ret;
 }
 
@@ -608,6 +622,14 @@ Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
   return theta_.retract(delta, ordering_);
 }
 
+/* ************************************************************************* */
+template<class CONDITIONAL, class GRAPH>
+const Permuted<VectorValues>& ISAM2<CONDITIONAL, GRAPH>::getDelta() const {
+  if(!deltaUptodate_)
+    updateDelta();
+  return delta_;
+}
+
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {

gtsam/nonlinear/ISAM2.h

@@ -283,19 +283,42 @@ protected:
   /** The linear delta from the last linear solution, an update to the estimate in theta */
   VectorValues deltaUnpermuted_;
 
-  /** @brief The permutation through which the deltaUnpermuted_ is
+  /** The permutation through which the deltaUnpermuted_ is
    * referenced.
    *
    * Permuting Vector entries would be slow, so for performance we
    * instead maintain this permutation through which we access the linear delta
    * indirectly
+   *
+   * This is \c mutable because it is a "cached" variable - it is not updated
+   * until either requested with getDelta() or calculateEstimate(), or needed
+   * during update() to evaluate whether to relinearize variables.
    */
-  Permuted<VectorValues> delta_;
+  mutable Permuted<VectorValues> delta_;
+
+  /** Indicates whether the current delta is up-to-date, only used
+   * internally - delta will always be updated if necessary when it is
+   * requested with getDelta() or calculateEstimate().
+   *
+   * This is \c mutable because it is used internally to not update delta_
+   * until it is needed.
+   */
+  mutable bool deltaUptodate_;
+
+  /** A cumulative mask for the variables that were replaced and have not yet
+   * been updated in the linear solution delta_, this is only used internally,
+   * delta will always be updated if necessary when requested with getDelta()
+   * or calculateEstimate().
+   *
+   * This is \c mutable because it is used internally to not update delta_
+   * until it is needed.
+   */
+  mutable std::vector<bool> deltaReplacedMask_;
 
   /** All original nonlinear factors are stored here to use during relinearization */
   GRAPH nonlinearFactors_;
 
-  /** @brief The current elimination ordering Symbols to Index (integer) keys.
+  /** The current elimination ordering Symbols to Index (integer) keys.
    *
    * We keep it up to date as we add and reorder variables.
    */
@@ -305,7 +328,7 @@ protected:
   ISAM2Params params_;
 
   /** The current Dogleg Delta (trust region radius) */
-  boost::optional<double> doglegDelta_;
+  mutable boost::optional<double> doglegDelta_;
 
 private:
 #ifndef NDEBUG
@@ -337,6 +360,8 @@ public:
     newISAM2->variableIndex_ = variableIndex_;
     newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
     newISAM2->delta_ = delta_;
+    newISAM2->deltaUptodate_ = deltaUptodate_;
+    newISAM2->deltaReplacedMask_ = deltaReplacedMask_;
     newISAM2->nonlinearFactors_ = nonlinearFactors_;
     newISAM2->ordering_ = ordering_;
     newISAM2->params_ = params_;
@@ -404,7 +429,7 @@ public:
   Values calculateBestEstimate() const;
 
   /** Access the current delta, computed during the last call to update */
-  const Permuted<VectorValues>& getDelta() const { return delta_; }
+  const Permuted<VectorValues>& getDelta() const;
 
   /** Access the set of nonlinear factors */
   const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; }
@@ -416,7 +441,7 @@ public:
   size_t lastAffectedFactorCount;
   size_t lastAffectedCliqueCount;
   size_t lastAffectedMarkedCount;
-  size_t lastBacksubVariableCount;
+  mutable size_t lastBacksubVariableCount;
   size_t lastNnzTop;
 
   ISAM2Params params() const { return params_; }
@@ -433,6 +458,7 @@ private:
       const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
       const boost::optional<FastSet<size_t> >& constrainKeys, ISAM2Result& result);
   //	void linear_update(const GaussianFactorGraph& newFactors);
+  void updateDelta(bool forceFullSolve = false) const;
 
 }; // ISAM2
 

tests/testGaussianISAM2.cpp

@@ -47,6 +47,8 @@ TEST(ISAM2, AddVariables) {
 
   Permuted<VectorValues> delta(permutation, deltaUnpermuted);
 
+  vector<bool> replacedKeys(2, false);
+
   Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
 
   GaussianISAM2<>::Nodes nodes(2);
@@ -75,17 +77,20 @@ TEST(ISAM2, AddVariables) {
 
   Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
 
+  vector<bool> replacedKeysExpected(3, false);
+
   Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
 
   GaussianISAM2<>::Nodes nodesExpected(
           3, GaussianISAM2<>::sharedClique());
 
   // Expand initial state
-  GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes);
+  GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes);
 
   EXPECT(assert_equal(thetaExpected, theta));
   EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
   EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
+  EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
   EXPECT(assert_equal(orderingExpected, ordering));
 }