Caching linearized factors in iSAM2, improves speed when linearization is expensive

Merge remote-tracking branch 'svn/branches/iSAM2_cache_linearized' into trunk

Conflicts:
	.cproject

gtsam/linear/GaussianFactor.h

@@ -96,7 +96,7 @@ namespace gtsam {
      * to already be inverted.  This acts just as a change-of-name for each
      * variable.  The order of the variables within the factor is not changed.
      */
-    virtual void permuteWithInverse(const Permutation& inversePermutation) = 0;
+    virtual void permuteWithInverse(const Permutation& inversePermutation) { IndexFactor::permuteWithInverse(inversePermutation); }
 
   private:
     /** Serialization function */

gtsam/linear/GaussianFactorGraph.cpp

@@ -51,7 +51,8 @@ namespace gtsam {
 	void GaussianFactorGraph::permuteWithInverse(
 	    const Permutation& inversePermutation) {
 	  BOOST_FOREACH(const sharedFactor& factor, factors_) {
-	    factor->permuteWithInverse(inversePermutation);
+	    if(factor)
+	      factor->permuteWithInverse(inversePermutation);
     }
 	}
 

gtsam/linear/HessianFactor.h

@@ -270,14 +270,6 @@ namespace gtsam {
      * @return The linear term \f$ g \f$ */
     constColumn linearTerm() const;
 
-    /**
-     * Permutes the GaussianFactor, but for efficiency requires the permutation
-     * to already be inverted.  This acts just as a change-of-name for each
-     * variable.  The order of the variables within the factor is not changed.
-     */
-    virtual void permuteWithInverse(const Permutation& inversePermutation) {
-      IndexFactor::permuteWithInverse(inversePermutation); }
-
     // Friend unit test classes
     friend class ::ConversionConstructorHessianFactorTest;
     friend class ::Constructor1HessianFactorTest;

gtsam/linear/JacobianFactor.cpp

@@ -250,43 +250,6 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
-  void JacobianFactor::permuteWithInverse(const Permutation& inversePermutation) {
-
-    // Build a map from the new variable indices to the old slot positions.
-    typedef FastMap<size_t, size_t> SourceSlots;
-    SourceSlots sourceSlots;
-    for(size_t j=0; j<size(); ++j)
-      sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
-
-    // Build a vector of variable dimensions in the new order
-    vector<size_t> dimensions(size() + 1);
-    size_t j = 0;
-    BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
-      dimensions[j++] = Ab_(sourceSlot.second).cols();
-    }
-    assert(j == size());
-    dimensions.back() = 1;
-
-    // Copy the variables and matrix into the new order
-    vector<Index> oldKeys(size());
-    keys_.swap(oldKeys);
-    AbMatrix oldMatrix;
-    BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows());
-    Ab_.swap(oldAb);
-    j = 0;
-    BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
-      keys_[j] = sourceSlot.first;
-      Ab_(j++).noalias() = oldAb(sourceSlot.second);
-    }
-    Ab_(j).noalias() = oldAb(j);
-
-    // Since we're permuting the variables, ensure that entire rows from this
-    // factor are copied when Combine is called
-    BOOST_FOREACH(size_t& varpos, firstNonzeroBlocks_) { varpos = 0; }
-    assertInvariants();
-  }
-
   /* ************************************************************************* */
   Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
     Vector e = -getb();

gtsam/linear/JacobianFactor.h

@@ -169,13 +169,6 @@ namespace gtsam {
      */
     virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); }
 
-    /**
-     * Permutes the GaussianFactor, but for efficiency requires the permutation
-     * to already be inverted.  This acts just as a change-of-name for each
-     * variable.  The order of the variables within the factor is not changed.
-     */
-    virtual void permuteWithInverse(const Permutation& inversePermutation);
-
     /**
      * return the number of rows in the corresponding linear system
      */

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -552,7 +552,7 @@ TEST(GaussianFactor, permuteWithInverse)
   actual.permuteWithInverse(inversePermutation);
 //  actualIndex.permute(*inversePermutation.inverse());
 
-  JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
+  JacobianFactor expected(4, A1, 2, A2, 0, A3, b, sharedSigma(2, 1.0));
   GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected)));
 //  GaussianVariableIndex expectedIndex(expectedFG);
 

gtsam/nonlinear/ISAM2-impl.cpp

@@ -257,6 +257,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
     cout << "Full var-ordered eliminated BT:\n";
     result.bayesTree->printTree("");
   }
+  // Undo permutation on our subset of cached factors, we must later permute *all* of the cached factors
+  factors.permuteWithInverse(*affectedColamd);
+  factors.permuteWithInverse(affectedKeysSelector);
   toc(8,"permute eliminated");
 
   return result;

gtsam/nonlinear/ISAM2.cpp

@@ -78,6 +78,12 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) {
   deltaUptodate_ = rhs.deltaUptodate_;
   deltaReplacedMask_ = rhs.deltaReplacedMask_;
   nonlinearFactors_ = rhs.nonlinearFactors_;
+
+  linearFactors_ = GaussianFactorGraph();
+  linearFactors_.reserve(rhs.linearFactors_.size());
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) {
+    linearFactors_.push_back(linearFactor->clone()); }
+
   ordering_ = rhs.ordering_;
   params_ = rhs.params_;
   doglegDelta_ = rhs.doglegDelta_;
@@ -125,7 +131,7 @@ FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
 // retrieve all factors that ONLY contain the affected variables
 // (note that the remaining stuff is summarized in the cached factors)
 FactorGraph<GaussianFactor>::shared_ptr
-ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
+ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const {
 
   tic(1,"getAffectedFactors");
   FastList<size_t> candidates = getAffectedFactors(affectedKeys);
@@ -139,24 +145,37 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
   affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
   toc(2,"affectedKeysSet");
 
-  tic(3,"check candidates");
+  tic(3,"check candidates and linearize");
+  FactorGraph<GaussianFactor>::shared_ptr linearized = boost::make_shared<FactorGraph<GaussianFactor> >();
   BOOST_FOREACH(size_t idx, candidates) {
     bool inside = true;
+    bool useCachedLinear = params_.cacheLinearizedFactors;
     BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
       Index var = ordering_[key];
-      if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
+      if(affectedKeysSet.find(var) == affectedKeysSet.end()) {
         inside = false;
         break;
       }
+      if(useCachedLinear && relinKeys.find(var) != relinKeys.end())
+        useCachedLinear = false;
+    }
+    if(inside) {
+      if(useCachedLinear) {
+        assert(linearFactors_[idx]);
+        linearized->push_back(linearFactors_[idx]);
+        assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys());
+      } else {
+        GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_);
+        linearized->push_back(linearFactor);
+        if(params_.cacheLinearizedFactors) {
+          assert(linearFactors_[idx]->keys() == linearFactor->keys());
+          linearFactors_[idx] = linearFactor;
+        }
+      }
     }
-    if (inside)
-      nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
   }
-  toc(3,"check candidates");
+  toc(3,"check candidates and linearize");
 
-  tic(4,"linearize");
-  FactorGraph<GaussianFactor>::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_));
-  toc(4,"linearize");
   return linearized;
 }
 
@@ -187,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
 }
 
 boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
-    const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
+    const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& newKeys,
     const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
 
   // TODO:  new factors are linearized twice, the newFactors passed in are not used.
@@ -300,11 +319,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     toc(1,"reorder");
 
     tic(2,"linearize");
-    GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_));
+    linearFactors_ = *nonlinearFactors_.linearize(theta_, ordering_);
     toc(2,"linearize");
 
     tic(5,"eliminate");
-    JunctionTree<GaussianFactorGraph, Base::Clique> jt(factors, variableIndex_);
+    JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearFactors_, variableIndex_);
     sharedClique newRoot;
     if(params_.factorization == ISAM2Params::LDL)
       newRoot = jt.eliminate(EliminatePreferLDL);
@@ -325,7 +344,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
 
     lastAffectedMarkedCount = markedKeys.size();
     lastAffectedVariableCount = affectedKeysSet->size();
-    lastAffectedFactorCount = factors.size();
+    lastAffectedFactorCount = linearFactors_.size();
 
     return affectedKeysSet;
 
@@ -338,7 +357,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end());
     affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end());
     tic(1,"relinearizeAffected");
-    GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys));
+    GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys));
     if(debug) factors.print("Relinearized factors: ");
     toc(1,"relinearizeAffected");
 
@@ -360,11 +379,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     // add the cached intermediate results from the boundary of the orphans ...
     GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans);
     if(debug) cachedBoundary.print("Boundary factors: ");
-    factors.reserve(factors.size() + cachedBoundary.size());
+    factors.push_back(cachedBoundary);
-    // Copy so that we can later permute factors
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) {
-      factors.push_back(cached->clone());
-    }
     toc(2,"cached");
 
     // END OF COPIED CODE
@@ -410,6 +425,15 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     tic(5,"permute ordering");
     ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
     toc(5,"permute ordering");
+    if(params_.cacheLinearizedFactors) {
+      tic(6,"permute cached linear");
+      //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
+      FastList<size_t> permuteLinearIndices = getAffectedFactors(affectedAndNewKeys);
+      BOOST_FOREACH(size_t idx, permuteLinearIndices) {
+        linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse);
+      }
+      toc(6,"permute cached linear");
+    }
 
     toc(4,"reorder and eliminate");
 
@@ -526,11 +550,12 @@ ISAM2Result ISAM2::update(
   toc(4,"gather involved keys");
 
   // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
+  FastSet<Index> relinKeys;
   if (relinearizeThisStep) {
     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);
+    relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
     if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging
 
     // Add the variables being relinearized to the marked keys
@@ -563,15 +588,21 @@ ISAM2Result ISAM2::update(
   }
 
   tic(8,"linearize new");
-  tic(1,"linearize");
   // 7. Linearize new factors
-  FactorGraph<GaussianFactor>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
+  if(params_.cacheLinearizedFactors) {
-  toc(1,"linearize");
+    tic(1,"linearize");
+    FactorGraph<GaussianFactor>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
+    linearFactors_.push_back(*linearFactors);
+    assert(nonlinearFactors_.size() == linearFactors_.size());
+    toc(1,"linearize");
 
-  tic(2,"augment VI");
+    tic(2,"augment VI");
-  // Augment the variable index with the new factors
+    // Augment the variable index with the new factors
-  variableIndex_.augment(*linearFactors);
+    variableIndex_.augment(*linearFactors);
-  toc(2,"augment VI");
+    toc(2,"augment VI");
+  } else {
+    variableIndex_.augment(*newFactors.symbolic(ordering_));
+  }
   toc(8,"linearize new");
 
   tic(9,"recalculate");
@@ -587,7 +618,7 @@ ISAM2Result ISAM2::update(
   }
   boost::shared_ptr<FastSet<Index> > replacedKeys;
   if(!markedKeys.empty() || !newKeys.empty())
-    replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result);
+    replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result);
 
   // Update replaced keys mask (accumulates until back-substitution takes place)
   if(replacedKeys) {

gtsam/nonlinear/ISAM2.h

@@ -118,6 +118,13 @@ struct ISAM2Params {
    */
   Factorization factorization;
 
+  /** Whether to cache linear factors (default: true).
+   * This can improve performance if linearization is expensive, but can hurt
+   * performance if linearization is very cleap due to computation to look up
+   * additional keys.
+   */
+  bool cacheLinearizedFactors;
+
   KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
 
   /** Specify parameters as constructor arguments */
@@ -128,11 +135,12 @@ struct ISAM2Params {
       bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
       bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
       Factorization _factorization = ISAM2Params::LDL, ///< see ISAM2Params::factorization
+      bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors
       const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
   ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
       relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
       evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
-      keyFormatter(_keyFormatter) {}
+      cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {}
 };
 
 /**
@@ -341,6 +349,9 @@ protected:
   /** All original nonlinear factors are stored here to use during relinearization */
   NonlinearFactorGraph nonlinearFactors_;
 
+  /** The current linear factors, which are only updated as needed */
+  mutable GaussianFactorGraph linearFactors_;
+
   /** The current elimination ordering Symbols to Index (integer) keys.
    *
    * We keep it up to date as we add and reorder variables.
@@ -453,11 +464,11 @@ public:
 private:
 
   FastList<size_t> getAffectedFactors(const FastList<Index>& keys) const;
-  FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const;
+  FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const;
   GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
 
-  boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys,
+  boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
-      const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
+      const FastVector<Index>& newKeys,
       const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
   //	void linear_update(const GaussianFactorGraph& newFactors);
   void updateDelta(bool forceFullSolve = false) const;