First pass at solution pointer caching in ISAM2 wildfire

gtsam/linear/linearAlgorithms-inst.h

@@ -30,6 +30,7 @@ namespace gtsam
       /* ************************************************************************* */
       struct OptimizeData {
         boost::optional<OptimizeData&> parentData;
+        //FastMap<Key, VectorValues::const_iterator> cliqueResults;
         //VectorValues ancestorResults;
         //VectorValues results;
       };
@@ -54,11 +55,28 @@ namespace gtsam
           myData.parentData = parentData;
           // Take any ancestor results we'll need
           //BOOST_FOREACH(Key parent, clique->conditional_->parents())
-          //  myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
+          //  myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent)));
           // Solve and store in our results
-          VectorValues result = clique->conditional()->solve(collectedResult/*myData.ancestorResults*/);
-          collectedResult.insert(result);
-          //myData.ancestorResults.insert(result);
+          collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/));
+          //{
+          //  GaussianConditional& c = *clique->conditional();
+          //  // Solve matrix
+          //  Vector xS = x.vector(vector<Key>(c.beginParents(), c.endParents()));
+          //  xS = c.getb() - c.get_S() * xS;
+          //  Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
+
+          //  // Check for indeterminant solution
+          //  if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
+
+          //  // Insert solution into a VectorValues
+          //  DenseIndex vectorPosition = 0;
+          //  for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
+          //    VectorValues::const_iterator r =
+          //      collectedResult.insert(*frontal, soln.segment(vectorPosition, c.getDim(frontal)));
+          //    myData.cliqueResults.insert(r->first, r);
+          //    vectorPosition += c.getDim(frontal);
+          //  }
+          //}
           return myData;
         }
       };

gtsam/nonlinear/ISAM2-inl.h

@@ -149,8 +149,63 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
       originalValues[it - clique->conditional()->beginFrontals()] = delta[*it];
     }
 
-    // Back-substitute
-    delta.update(clique->conditional()->solve(delta));
+    // Back-substitute - special version stores solution pointers in cliques for fast access.
+    {
+      // Create solution part pointers if necessary and possible - necessary if solnPointers_ is
+      // empty, and possible if either we're a root, or we have a parent with valid solnPointers_.
+      boost::shared_ptr<CLIQUE> parent = clique->parent_.lock();
+      if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty()))
+      {
+        BOOST_FOREACH(Key key, clique->conditional()->frontals())
+          clique->solnPointers_.insert(std::make_pair(key, delta.find(key)));
+        BOOST_FOREACH(Key key, clique->conditional()->parents())
+          clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key)));
+      }
+
+      // See if we can use solution part pointers - we can if they either already existed or were
+      // created above.
+      if(!clique->solnPointers_.empty())
+      {
+        GaussianConditional& c = *clique->conditional();
+        // Solve matrix
+        Vector xS;
+        {
+          // Count dimensions of vector
+          DenseIndex dim = 0;
+          vector<VectorValues::const_iterator> parentPointers;
+          parentPointers.reserve(clique->conditional()->nrParents());
+          BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
+            parentPointers.push_back(clique->solnPointers_.at(parent));
+            dim += parentPointers.back()->second.size();
+          }
+
+          // Fill parent vector
+          xS.resize(dim);
+          DenseIndex vectorPos = 0;
+          BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) {
+            xS.segment(vectorPos, parentPointer->second.size()) = parentPointer->second;
+            vectorPos += parentPointer->second.size();
+          }
+        }
+        xS = c.getb() - c.get_S() * xS;
+        Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
+
+        // Check for indeterminant solution
+        if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
+
+        // Insert solution into a VectorValues
+        DenseIndex vectorPosition = 0;
+        for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
+          clique->solnPointers_.at(*frontal)->second = soln.segment(vectorPosition, c.getDim(frontal));
+          vectorPosition += c.getDim(frontal);
+        }
+      }
+      else
+      {
+        // Just call plain solve because we couldn't use solution pointers.
+        delta.update(clique->conditional()->solve(delta));
+      }
+    }
     count += clique->conditional()->nrFrontals();
 
     // Whether the values changed above a threshold, or always true if the

gtsam/nonlinear/ISAM2.h

@@ -360,6 +360,23 @@ public:
 
   Base::FactorType::shared_ptr cachedFactor_;
   Vector gradientContribution_;
+  FastMap<Key, VectorValues::iterator> solnPointers_;
+
+  /// Default constructor
+  ISAM2Clique() : Base() {}
+
+  /// Copy constructor, does *not* copy solution pointers as these are invalid in different trees.
+  ISAM2Clique(const ISAM2Clique& other) :
+    Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {}
+
+  /// Assignment operator, does *not* copy solution pointers as these are invalid in different trees.
+  ISAM2Clique& operator=(const ISAM2Clique& other)
+  {
+    Base::operator=(other);
+    cachedFactor_ = other.cachedFactor_;
+    gradientContribution_ = other.gradientContribution_;
+    return *this;
+  }
 
   /// Overridden to also store the remaining factor and gradient contribution
   void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult);