Storing variable index in solver, saved between nonlinear iterations

gtsam/inference/EliminationTree-inl.h

@@ -4,6 +4,7 @@
  * @author  Frank Dellaert
  * @created Oct 13, 2010
  */
+#pragma once
 
 #include <gtsam/base/timing.h>
 #include <gtsam/inference/EliminationTree.h>

gtsam/inference/GenericMultifrontalSolver-inl.h

@@ -33,14 +33,31 @@ namespace gtsam {
 /* ************************************************************************* */
 template<class FACTOR, class JUNCTIONTREE>
 GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) :
-    junctionTree_(factorGraph) {}
+    structure_(new VariableIndex(factorGraph)), junctionTree_(new JUNCTIONTREE(factorGraph, *structure_)) {}
+
+/* ************************************************************************* */
+template<class FACTOR, class JUNCTIONTREE>
+GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) :
+    structure_(new VariableIndex(*factorGraph)), junctionTree_(new JUNCTIONTREE(*factorGraph, *structure_)) {}
+
+/* ************************************************************************* */
+template<class FACTOR, class JUNCTIONTREE>
+GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(
+    const typename FactorGraph<FACTOR>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) :
+    structure_(variableIndex), junctionTree_(new JUNCTIONTREE(*factorGraph, *structure_)) {}
+
+/* ************************************************************************* */
+template<class FACTOR, class JUNCTIONTREE>
+void GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) {
+  junctionTree_.reset(new JUNCTIONTREE(*factorGraph, *structure_));
+}
 
 /* ************************************************************************* */
 template<class FACTOR, class JUNCTIONTREE>
 typename JUNCTIONTREE::BayesTree::shared_ptr
 GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate() const {
   typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree(new typename JUNCTIONTREE::BayesTree);
-  bayesTree->insert(junctionTree_.eliminate());
+  bayesTree->insert(junctionTree_->eliminate());
   return bayesTree;
 }
 

gtsam/inference/GenericMultifrontalSolver.h

@@ -31,17 +31,40 @@ class GenericMultifrontalSolver {
 
 protected:
 
+  // Column structure of the factor graph
+  VariableIndex::shared_ptr structure_;
+
   // Junction tree that performs elimination.
-  JUNCTIONTREE junctionTree_;
+  typename JUNCTIONTREE::shared_ptr junctionTree_;
 
 public:
 
   /**
-   * Construct the solver for a factor graph.  This builds the elimination
+   * Construct the solver for a factor graph.  This builds the junction
-   * tree, which already does some of the symbolic work of elimination.
+   * tree, which already does some of the work of elimination.
    */
   GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph);
 
+  /**
+   * Construct the solver for a factor graph.  This builds the junction
+   * tree, which already does some of the work of elimination.
+   */
+  GenericMultifrontalSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
+
+  /**
+   * Construct the solver with a shared pointer to a factor graph and to a
+   * VariableIndex.  The solver will store these pointers, so this constructor
+   * is the fastest.
+   */
+  GenericMultifrontalSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
+
+  /**
+   * Replace the factor graph with a new one having the same structure.  The
+   * This function can be used if the numerical part of the factors changes,
+   * such as during relinearization or adjusting of noise models.
+   */
+  void replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
+
   /**
    * Eliminate the factor graph sequentially.  Uses a column elimination tree
    * to recursively eliminate.

gtsam/inference/GenericSequentialSolver-inl.h

@@ -31,9 +31,30 @@ namespace gtsam {
 /* ************************************************************************* */
 template<class FACTOR>
 GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) :
-    structure_(factorGraph),
+    factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(new VariableIndex(factorGraph)),
-    eliminationTree_(EliminationTree<FACTOR>::Create(factorGraph, structure_)) {
+    eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
-  factors_.push_back(factorGraph);
+
+/* ************************************************************************* */
+template<class FACTOR>
+GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) :
+    factors_(factorGraph), structure_(new VariableIndex(*factorGraph)),
+    eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
+
+/* ************************************************************************* */
+template<class FACTOR>
+GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph,
+    const VariableIndex::shared_ptr& variableIndex) :
+    factors_(factorGraph), structure_(variableIndex),
+    eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
+
+/* ************************************************************************* */
+template<class FACTOR>
+void GenericSequentialSolver<FACTOR>::replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) {
+  // Reset this shared pointer first to deallocate if possible - for big
+  // problems there may not be enough memory to store two copies.
+  eliminationTree_.reset();
+  factors_ = factorGraph;
+  eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
 }
 
 /* ************************************************************************* */
@@ -47,23 +68,23 @@ template<class FACTOR>
 typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::jointFactorGraph(const std::vector<Index>& js) const {
 
   // Compute a COLAMD permutation with the marginal variable constrained to the end.
-  Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(structure_, js));
+  Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js));
   Permutation::shared_ptr permutationInverse(permutation->inverse());
 
   // Permute the factors - NOTE that this permutes the original factors, not
   // copies.  Other parts of the code may hold shared_ptr's to these factors so
   // we must undo the permutation before returning.
-  BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) {
+  BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) {
     if(factor)
       factor->permuteWithInverse(*permutationInverse);
   }
 
   // Eliminate all variables
   typename BayesNet<typename FACTOR::Conditional>::shared_ptr bayesNet(
-      EliminationTree<FACTOR>::Create(factors_)->eliminate());
+      EliminationTree<FACTOR>::Create(*factors_)->eliminate());
 
   // Undo the permuation on the original factors and on the structure.
-  BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) {
+  BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) {
     if(factor)
       factor->permuteWithInverse(*permutation);
   }

gtsam/inference/GenericSequentialSolver.h

@@ -32,10 +32,10 @@ class GenericSequentialSolver {
 protected:
 
   // Store the original factors for computing marginals
-  FactorGraph<FACTOR> factors_;
+  typename FactorGraph<FACTOR>::shared_ptr factors_;
 
   // Column structure of the factor graph
-  VariableIndex structure_;
+  VariableIndex::shared_ptr structure_;
 
   // Elimination tree that performs elimination.
   typename EliminationTree<FACTOR>::shared_ptr eliminationTree_;
@@ -44,10 +44,30 @@ public:
 
   /**
    * Construct the solver for a factor graph.  This builds the elimination
-   * tree, which already does some of the symbolic work of elimination.
+   * tree, which already does some of the work of elimination.
    */
   GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph);
 
+  /**
+   * Construct the solver for a factor graph.  This builds the elimination
+   * tree, which already does some of the work of elimination.
+   */
+  GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
+
+  /**
+   * Construct the solver with a shared pointer to a factor graph and to a
+   * VariableIndex.  The solver will store these pointers, so this constructor
+   * is the fastest.
+   */
+  GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
+
+  /**
+   * Replace the factor graph with a new one having the same structure.  The
+   * This function can be used if the numerical part of the factors changes,
+   * such as during relinearization or adjusting of noise models.
+   */
+  void replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
+
   /**
    * Eliminate the factor graph sequentially.  Uses a column elimination tree
    * to recursively eliminate.

gtsam/inference/JunctionTree-inl.h

@@ -25,7 +25,7 @@
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/inference/inference-inl.h>
 #include <gtsam/inference/VariableSlots.h>
-#include <gtsam/inference/SymbolicSequentialSolver.h>
+#include <gtsam/inference/EliminationTree-inl.h>
 #include <gtsam/inference/ClusterTree-inl.h>
 
 #include <boost/foreach.hpp>
@@ -35,129 +35,136 @@
 
 namespace gtsam {
 
-	using namespace std;
+  using namespace std;
-
-	/* ************************************************************************* */
-	template <class FG>
-	JunctionTree<FG>::JunctionTree(const FG& fg) {
-	  tic("JT 1  constructor");
-		// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph
-		// -> SymbolicBayesNet -> SymbolicBayesTree
-		tic("JT 1.1  symbolic elimination");
-		SymbolicBayesNet::shared_ptr sbn = SymbolicSequentialSolver(fg).eliminate();
-//		SymbolicFactorGraph sfg(fg);
-//		SymbolicBayesNet::shared_ptr sbn_orig = Inference::Eliminate(sfg);
-//		assert(assert_equal(*sbn, *sbn_orig));
-    toc("JT 1.1  symbolic elimination");
-    tic("JT 1.2  symbolic BayesTree");
-		SymbolicBayesTree sbt(*sbn);
-		toc("JT 1.2  symbolic BayesTree");
-
-		// distribute factors
-    tic("JT 1.3  distributeFactors");
-		this->root_ = distributeFactors(fg, sbt.root());
-    toc("JT 1.3  distributeFactors");
-		toc("JT 1  constructor");
-	}
-
-	/* ************************************************************************* */
-	template<class FG>
-	typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(
-			const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) {
-
-	  // Build "target" index.  This is an index for each variable of the factors
-	  // that involve this variable as their *lowest-ordered* variable.  For each
-	  // factor, it is the lowest-ordered variable of that factor that pulls the
-	  // factor into elimination, after which all of the information in the
-	  // factor is contained in the eliminated factors that are passed up the
-	  // tree as elimination continues.
-
-	  // Two stages - first build an array of the lowest-ordered variable in each
-	  // factor and find the last variable to be eliminated.
-	  vector<Index> lowestOrdered(fg.size());
-	  Index maxVar = 0;
-	  for(size_t i=0; i<fg.size(); ++i)
-	    if(fg[i]) {
-	      typename FG::Factor::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
-	      if(min == fg[i]->end())
-	        lowestOrdered[i] = numeric_limits<Index>::max();
-	      else {
-	        lowestOrdered[i] = *min;
-	        maxVar = std::max(maxVar, *min);
-	      }
-	    }
-
-	  // Now add each factor to the list corresponding to its lowest-ordered
-	  // variable.
-	  vector<list<size_t, boost::fast_pool_allocator<size_t> > > targets(maxVar+1);
-	  for(size_t i=0; i<lowestOrdered.size(); ++i)
-	    if(lowestOrdered[i] != numeric_limits<Index>::max())
-	      targets[lowestOrdered[i]].push_back(i);
-
-	  // Now call the recursive distributeFactors
-	  return distributeFactors(fg, targets, bayesClique);
-	}
 
   /* ************************************************************************* */
-	template<class FG>
+  template <class FG>
-	typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(const FG& fg,
+  void JunctionTree<FG>::construct(const FG& fg, const VariableIndex& variableIndex) {
-	    const std::vector<std::list<size_t,boost::fast_pool_allocator<size_t> > >& targets,
+    tic("JT 1  constructor");
+    tic("JT 1.1  symbolic elimination");
+    SymbolicBayesNet::shared_ptr sbn = EliminationTree<IndexFactor>::Create(fg, variableIndex)->eliminate();
+    toc("JT 1.1  symbolic elimination");
+    tic("JT 1.2  symbolic BayesTree");
+    SymbolicBayesTree sbt(*sbn);
+    toc("JT 1.2  symbolic BayesTree");
+
+    // distribute factors
+    tic("JT 1.3  distributeFactors");
+    this->root_ = distributeFactors(fg, sbt.root());
+    toc("JT 1.3  distributeFactors");
+    toc("JT 1  constructor");
+  }
+
+  /* ************************************************************************* */
+  template <class FG>
+  JunctionTree<FG>::JunctionTree(const FG& fg) {
+    construct(fg, VariableIndex(fg));
+  }
+
+  /* ************************************************************************* */
+  template <class FG>
+  JunctionTree<FG>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
+    construct(fg, variableIndex);
+  }
+
+  /* ************************************************************************* */
+  template<class FG>
+  typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(
+      const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) {
+
+    // Build "target" index.  This is an index for each variable of the factors
+    // that involve this variable as their *lowest-ordered* variable.  For each
+    // factor, it is the lowest-ordered variable of that factor that pulls the
+    // factor into elimination, after which all of the information in the
+    // factor is contained in the eliminated factors that are passed up the
+    // tree as elimination continues.
+
+    // Two stages - first build an array of the lowest-ordered variable in each
+    // factor and find the last variable to be eliminated.
+    vector<Index> lowestOrdered(fg.size());
+    Index maxVar = 0;
+    for(size_t i=0; i<fg.size(); ++i)
+      if(fg[i]) {
+        typename FG::Factor::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
+        if(min == fg[i]->end())
+          lowestOrdered[i] = numeric_limits<Index>::max();
+        else {
+          lowestOrdered[i] = *min;
+          maxVar = std::max(maxVar, *min);
+        }
+      }
+
+    // Now add each factor to the list corresponding to its lowest-ordered
+    // variable.
+    vector<list<size_t, boost::fast_pool_allocator<size_t> > > targets(maxVar+1);
+    for(size_t i=0; i<lowestOrdered.size(); ++i)
+      if(lowestOrdered[i] != numeric_limits<Index>::max())
+        targets[lowestOrdered[i]].push_back(i);
+
+    // Now call the recursive distributeFactors
+    return distributeFactors(fg, targets, bayesClique);
+  }
+
+  /* ************************************************************************* */
+  template<class FG>
+  typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(const FG& fg,
+      const std::vector<std::list<size_t,boost::fast_pool_allocator<size_t> > >& targets,
       const SymbolicBayesTree::sharedClique& bayesClique) {
 
-	  if(bayesClique) {
+    if(bayesClique) {
-	    // create a new clique in the junction tree
+      // create a new clique in the junction tree
-	    list<Index> frontals = bayesClique->ordering();
+      list<Index> frontals = bayesClique->ordering();
-	    sharedClique clique(new Clique(frontals.begin(), frontals.end(), bayesClique->separator_.begin(), bayesClique->separator_.end()));
+      sharedClique clique(new Clique(frontals.begin(), frontals.end(), bayesClique->separator_.begin(), bayesClique->separator_.end()));
 
-	    // count the factors for this cluster to pre-allocate space
+      // count the factors for this cluster to pre-allocate space
-	    {
+      {
-	      size_t nFactors = 0;
+        size_t nFactors = 0;
-	      BOOST_FOREACH(const Index frontal, clique->frontal) {
+        BOOST_FOREACH(const Index frontal, clique->frontal) {
-	        // There may be less variables in "targets" than there really are if
+          // There may be less variables in "targets" than there really are if
-	        // some of the highest-numbered variables do not pull in any factors.
+          // some of the highest-numbered variables do not pull in any factors.
-	        if(frontal < targets.size())
+          if(frontal < targets.size())
-	          nFactors += targets[frontal].size(); }
+            nFactors += targets[frontal].size(); }
-	      clique->reserve(nFactors);
+        clique->reserve(nFactors);
-	    }
+      }
-	    // add the factors to this cluster
+      // add the factors to this cluster
-	    BOOST_FOREACH(const Index frontal, clique->frontal) {
+      BOOST_FOREACH(const Index frontal, clique->frontal) {
-	      if(frontal < targets.size()) {
+        if(frontal < targets.size()) {
-	        BOOST_FOREACH(const size_t factorI, targets[frontal]) {
+          BOOST_FOREACH(const size_t factorI, targets[frontal]) {
-	          clique->push_back(fg[factorI]); } } }
+            clique->push_back(fg[factorI]); } } }
 
-	    // recursively call the children
+      // recursively call the children
-	    BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
+      BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
-	      sharedClique child = distributeFactors(fg, targets, bayesChild);
+        sharedClique child = distributeFactors(fg, targets, bayesChild);
-	      clique->addChild(child);
+        clique->addChild(child);
-	      child->parent() = clique;
+        child->parent() = clique;
-	    }
+      }
-	    return clique;
+      return clique;
-	  } else
+    } else
-	    return sharedClique();
+      return sharedClique();
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class FG>
+  template <class FG>
-	pair<typename JunctionTree<FG>::BayesTree::sharedClique, typename FG::sharedFactor>
+  pair<typename JunctionTree<FG>::BayesTree::sharedClique, typename FG::sharedFactor>
-	JunctionTree<FG>::eliminateOneClique(const boost::shared_ptr<const Clique>& current) const {
+  JunctionTree<FG>::eliminateOneClique(const boost::shared_ptr<const Clique>& current) const {
 
-		FG fg; // factor graph will be assembled from local factors and marginalized children
+    FG fg; // factor graph will be assembled from local factors and marginalized children
-		fg.reserve(current->size() + current->children().size());
+    fg.reserve(current->size() + current->children().size());
-		fg.push_back(*current); // add the local factors
+    fg.push_back(*current); // add the local factors
 
     // receive the factors from the child and its clique point
     list<typename BayesTree::sharedClique> children;
-		BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
+    BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
-		  pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor(
+      pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor(
-		      eliminateOneClique(child));
+          eliminateOneClique(child));
       children.push_back(tree_factor.first);
-			fg.push_back(tree_factor.second);
+      fg.push_back(tree_factor.second);
-		}
+    }
 
-		// eliminate the combined factors
+    // eliminate the combined factors
-		// warning: fg is being eliminated in-place and will contain marginal afterwards
+    // warning: fg is being eliminated in-place and will contain marginal afterwards
-		tic("JT 2.1 VariableSlots");
+    tic("JT 2.1 VariableSlots");
-		VariableSlots variableSlots(fg);
+    VariableSlots variableSlots(fg);
     toc("JT 2.1 VariableSlots");
 #ifndef NDEBUG
     // Debug check that the keys found in the factors match the frontal and
@@ -182,29 +189,29 @@ namespace gtsam {
     assert(std::equal(jointFactor->begin(), jointFactor->end(), current->separator.begin()));
 
     tic("JT 2.4 Update tree");
-		// create a new clique corresponding the combined factors
+    // create a new clique corresponding the combined factors
-		typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(*fragment));
+    typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(*fragment));
-		new_clique->children_ = children;
+    new_clique->children_ = children;
 
-		BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children)
+    BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children)
-			childRoot->parent_ = new_clique;
+    childRoot->parent_ = new_clique;
 
     toc("JT 2.4 Update tree");
-		return make_pair(new_clique, jointFactor);
+    return make_pair(new_clique, jointFactor);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class FG>
+  template <class FG>
-	typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate() const {
+  typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate() const {
-	  if(this->root()) {
+    if(this->root()) {
-	    tic("JT 2 eliminate");
+      tic("JT 2 eliminate");
-	    pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = this->eliminateOneClique(this->root());
+      pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = this->eliminateOneClique(this->root());
-	    if (ret.second->size() != 0)
+      if (ret.second->size() != 0)
-	      throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
+        throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
-	    toc("JT 2 eliminate");
+      toc("JT 2 eliminate");
-	    return ret.first;
+      return ret.first;
-	  } else
+    } else
-	    return typename BayesTree::sharedClique();
+      return typename BayesTree::sharedClique();
-	}
+  }
 
 } //namespace gtsam

gtsam/inference/JunctionTree.h

@@ -48,6 +48,8 @@ namespace gtsam {
 
 		typedef class BayesTree<typename FG::Factor::Conditional> BayesTree;
 
+		typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
+
 		// And we will frequently refer to a symbolic Bayes tree
 		typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
 
@@ -64,14 +66,19 @@ namespace gtsam {
 		std::pair<typename BayesTree::sharedClique, typename FG::sharedFactor>
 		eliminateOneClique(const boost::shared_ptr<const Clique>& clique) const;
 
-	public:
+		// internal constructor
-		// constructor
+		void construct(const FG& fg, const VariableIndex& variableIndex);
-		JunctionTree() {
-		}
 
-		// constructor given a factor graph and the elimination ordering
+	public:
+		/** Default constructor */
+		JunctionTree() {}
+
+		/** Construct from a factor graph.  Computes a variable index. */
 		JunctionTree(const FG& fg);
 
+		/** Construct from a factor graph and a pre-computed variable index. */
+		JunctionTree(const FG& fg, const VariableIndex& variableIndex);
+
 		// eliminate the factors in the subgraphs
 		typename BayesTree::sharedClique eliminate() const;
 

gtsam/inference/tests/testJunctionTree.cpp

@@ -31,6 +31,7 @@ using namespace boost::assign;
 #include <gtsam/inference/ClusterTree-inl.h>
 #include <gtsam/inference/JunctionTree-inl.h>
 #include <gtsam/inference/IndexFactor.h>
+#include <gtsam/inference/SymbolicSequentialSolver.h>
 
 using namespace gtsam;
 

gtsam/linear/GaussianFactorGraph.cpp

@@ -146,24 +146,6 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
 	return fg;
 }
 
-/* ************************************************************************* */  
-GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector<size_t>& dimensions) const {
-
-	// start with this factor graph
-	GaussianFactorGraph result = *this;
-
-	// for each of the variables, add a prior
-	for(Index j=0; j<dimensions.size(); ++j) {
-	  size_t dim = dimensions[j];
-		Matrix A = eye(dim);
-		Vector b = zero(dim);
-		SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
-		sharedFactor prior(new GaussianFactor(j, A, b, model));
-		result.push_back(prior);
-	}
-	return result;
-}
-
 bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const {
 
 	//typedef sharedFactor F ;

gtsam/linear/GaussianFactorGraph.h

@@ -151,13 +151,6 @@ namespace gtsam {
      */
     void combine(const GaussianFactorGraph &lfg);
 
-
-    /**
-     * Add zero-mean i.i.d. Gaussian prior terms to each variable
-     * @param sigma Standard deviation of Gaussian
-     */
-    GaussianFactorGraph add_priors(double sigma, const std::vector<size_t>& dimensions) const;
-
 	/**
 	 * Split a Gaussian factor graph into two, according to M
 	 * M keeps the vertex indices of edges of A1. The others belong to A2.

gtsam/linear/GaussianJunctionTree.h

@@ -43,11 +43,15 @@ namespace gtsam {
 
 	public :
 
+		/** Default constructor */
 		GaussianJunctionTree() : Base() {}
 
-		// constructor
+		/** Constructor from a factor graph.  Builds a VariableIndex. */
 		GaussianJunctionTree(const GaussianFactorGraph& fg) : Base(fg) {}
 
+    /** Construct from a factor graph and a pre-computed variable index. */
+    GaussianJunctionTree(const GaussianFactorGraph& fg, const VariableIndex& variableIndex) : Base(fg, variableIndex) {}
+
 		// optimize the linear graph
 		VectorValues optimize() const;
 	}; // GaussianJunctionTree

gtsam/linear/GaussianMultifrontalSolver.cpp

@@ -31,17 +31,23 @@ GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<Gaussia
     Base(factorGraph) {}
 
 /* ************************************************************************* */
-GaussianMultifrontalSolver::shared_ptr
+GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) :
-GaussianMultifrontalSolver::Create(const FactorGraph<GaussianFactor>& factorGraph) {
+    Base(factorGraph) {}
-  return shared_ptr(new GaussianMultifrontalSolver(factorGraph));
+
-}
+/* ************************************************************************* */
+GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) :
+    Base(factorGraph, variableIndex) {}
 
 /* ************************************************************************* */
 GaussianMultifrontalSolver::shared_ptr
-GaussianMultifrontalSolver::update(const FactorGraph<GaussianFactor>& factorGraph) const {
+GaussianMultifrontalSolver::Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
-  // We do not yet have code written to update the junction tree, so we just
+    const VariableIndex::shared_ptr& variableIndex) {
-  // create a new solver.
+  return shared_ptr(new GaussianMultifrontalSolver(factorGraph, variableIndex));
-  return Create(factorGraph);
+}
+
+/* ************************************************************************* */
+void GaussianMultifrontalSolver::replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) {
+  Base::replaceFactors(factorGraph);
 }
 
 /* ************************************************************************* */
@@ -51,7 +57,7 @@ BayesTree<GaussianConditional>::shared_ptr GaussianMultifrontalSolver::eliminate
 
 /* ************************************************************************* */
 VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
-  return VectorValues::shared_ptr(new VectorValues(junctionTree_.optimize()));
+  return VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize()));
 }
 
 /* ************************************************************************* */

gtsam/linear/GaussianMultifrontalSolver.h

@@ -54,15 +54,28 @@ public:
 
   /**
    * Construct the solver for a factor graph.  This builds the elimination
-   * tree, which already does some of the symbolic work of elimination.
+   * tree, which already does some of the work of elimination.
    */
   GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph);
 
   /**
-   * Named constructor that returns a shared_ptr.  This builds the junction
+   * Construct the solver for a factor graph.  This builds the elimination
+   * tree, which already does some of the work of elimination.
+   */
+  GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
+
+  /**
+   * Construct the solver with a shared pointer to a factor graph and to a
+   * VariableIndex.  The solver will store these pointers, so this constructor
+   * is the fastest.
+   */
+  GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
+
+  /**
+   * Named constructor to return a shared_ptr.  This builds the elimination
    * tree, which already does some of the symbolic work of elimination.
    */
-  static shared_ptr Create(const FactorGraph<GaussianFactor>& factorGraph);
+  static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
 
   /**
    * Return a new solver that solves the given factor graph, which must have
@@ -71,7 +84,7 @@ public:
    * used in cases where the numerical values of the linear problem change,
    * e.g. during iterative nonlinear optimization.
    */
-  shared_ptr update(const FactorGraph<GaussianFactor>& factorGraph) const;
+  void replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
 
   /**
    * Eliminate the factor graph sequentially.  Uses a column elimination tree

gtsam/linear/GaussianSequentialSolver.cpp

@@ -31,15 +31,23 @@ GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFac
     Base(factorGraph) {}
 
 /* ************************************************************************* */
-GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create(const FactorGraph<GaussianFactor>& factorGraph) {
+GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) :
-  return shared_ptr(new GaussianSequentialSolver(factorGraph));
+    Base(factorGraph) {}
+
+/* ************************************************************************* */
+GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
+    const VariableIndex::shared_ptr& variableIndex) :
+    Base(factorGraph, variableIndex) {}
+
+/* ************************************************************************* */
+GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create(
+    const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) {
+  return shared_ptr(new GaussianSequentialSolver(factorGraph, variableIndex));
 }
 
 /* ************************************************************************* */
-GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::update(const FactorGraph<GaussianFactor>& factorGraph) const {
+void GaussianSequentialSolver::replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) {
-  // We do not yet have code written to update the elimination tree, so we just
+  Base::replaceFactors(factorGraph);
-  // create a new solver.
-  return Create(factorGraph);
 }
 
 /* ************************************************************************* */
@@ -52,7 +60,7 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
 
   static const bool debug = false;
 
-  if(debug) this->factors_.print("GaussianSequentialSolver, eliminating ");
+  if(debug) this->factors_->print("GaussianSequentialSolver, eliminating ");
   if(debug) this->eliminationTree_->print("GaussianSequentialSolver, elimination tree ");
 
   // Eliminate using the elimination tree

gtsam/linear/GaussianSequentialSolver.h

@@ -61,15 +61,28 @@ public:
 
   /**
    * Construct the solver for a factor graph.  This builds the elimination
-   * tree, which already does some of the symbolic work of elimination.
+   * tree, which already does some of the work of elimination.
    */
   GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph);
 
+  /**
+   * Construct the solver for a factor graph.  This builds the elimination
+   * tree, which already does some of the work of elimination.
+   */
+  GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
+
+  /**
+   * Construct the solver with a shared pointer to a factor graph and to a
+   * VariableIndex.  The solver will store these pointers, so this constructor
+   * is the fastest.
+   */
+  GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
+
   /**
    * Named constructor to return a shared_ptr.  This builds the elimination
    * tree, which already does some of the symbolic work of elimination.
    */
-  static shared_ptr Create(const FactorGraph<GaussianFactor>& factorGraph);
+  static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
 
   /**
    * Return a new solver that solves the given factor graph, which must have
@@ -78,7 +91,7 @@ public:
    * used in cases where the numerical values of the linear problem change,
    * e.g. during iterative nonlinear optimization.
    */
-  shared_ptr update(const FactorGraph<GaussianFactor>& factorGraph) const;
+  void replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
 
   /**
    * Eliminate the factor graph sequentially.  Uses a column elimination tree

gtsam/linear/SubgraphSolver-inl.h

@@ -27,14 +27,13 @@ using namespace std;
 namespace gtsam {
 
 template<class GRAPH, class LINEAR, class VALUES>
-typename SubgraphSolver<GRAPH,LINEAR,VALUES>::shared_ptr
+void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
-SubgraphSolver<GRAPH,LINEAR,VALUES>::update(const LINEAR &graph) const {
 
 	shared_linear Ab1 = boost::make_shared<LINEAR>(),
 				  Ab2 = boost::make_shared<LINEAR>();
 
 	if (parameters_->verbosity()) cout << "split the graph ...";
-	graph.split(pairs_, *Ab1, *Ab2) ;
+	graph->split(pairs_, *Ab1, *Ab2) ;
 	if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
 
 	//	// Add a HardConstraint to the root, otherwise the root will be singular
@@ -48,8 +47,7 @@ SubgraphSolver<GRAPH,LINEAR,VALUES>::update(const LINEAR &graph) const {
 	SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
 	SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
 
-	shared_preconditioner pc = boost::make_shared<SubgraphPreconditioner>(Ab1,Ab2,Rc1,xbar);
+	pc_ = boost::make_shared<SubgraphPreconditioner>(Ab1,Ab2,Rc1,xbar);
-	return boost::make_shared<SubgraphSolver>(ordering_, pairs_, pc, parameters_) ;
 }
 
 template<class GRAPH, class LINEAR, class VALUES>

gtsam/linear/SubgraphSolver.h

@@ -57,7 +57,7 @@ namespace gtsam {
 		SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters()):
 			IterativeSolver(parameters){ initialize(G,theta0); }
 
-	  	SubgraphSolver(const LINEAR &GFG) {
+	  	SubgraphSolver(const typename LINEAR::shared_ptr& GFG) {
 			std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
 			throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
 	  	}
@@ -71,7 +71,7 @@ namespace gtsam {
 	  				   sharedParameters parameters = boost::make_shared<Parameters>()) :
 	  		IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc) {}
 
-		shared_ptr update(const LINEAR &graph) const ;
+		void replaceFactors(const typename LINEAR::shared_ptr &graph);
 		VectorValues::shared_ptr optimize() const ;
 		shared_ordering ordering() const { return ordering_; }
 

gtsam/nonlinear/NonlinearOptimizer-inl.h

@@ -115,7 +115,7 @@ namespace gtsam {
 	    boost::shared_ptr<L> linearized = graph_->linearize(*values_, *ordering_);
 		shared_solver newSolver = solver_;
 
-		if(newSolver) newSolver = newSolver->update(*linearized);
+		if(newSolver) newSolver->replaceFactors(linearized);
 		else newSolver.reset(new S(*linearized));
 
 		VectorValues delta = *newSolver->optimize();
@@ -209,11 +209,24 @@ namespace gtsam {
 		  if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl;
 
 		  // add prior-factors
-		  L damped = linear.add_priors(1.0/sqrt(lambda), *dimensions_);
+		  typename L::shared_ptr damped(new L(linear));
-		  if (verbosity >= Parameters::DAMPED) damped.print("damped");
+		  {
+		    double sigma = 1.0 / sqrt(lambda);
+		    damped->reserve(damped->size() + dimensions_->size());
+		    // for each of the variables, add a prior
+		    for(Index j=0; j<dimensions_->size(); ++j) {
+		      size_t dim = (*dimensions_)[j];
+		      Matrix A = eye(dim);
+		      Vector b = zero(dim);
+		      SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
+		      GaussianFactor::shared_ptr prior(new GaussianFactor(j, A, b, model));
+		      damped->push_back(prior);
+		    }
+		  }
+		  if (verbosity >= Parameters::DAMPED) damped->print("damped");
 
 		  // solve
-		  if(solver_) solver_ = solver_->update(damped);
+		  if(solver_) solver_->replaceFactors(damped);
 		  else solver_.reset(new S(damped));
 
 		  VectorValues delta = *solver_->optimize();

gtsam/nonlinear/NonlinearOptimizer.h

@@ -71,7 +71,7 @@ namespace gtsam {
 		typedef boost::shared_ptr<const T> shared_values;
 		typedef boost::shared_ptr<const G> shared_graph;
 		typedef boost::shared_ptr<const Ordering> shared_ordering;
-		typedef boost::shared_ptr<const GS> shared_solver;
+		typedef boost::shared_ptr<GS> shared_solver;
 		typedef NonlinearOptimizationParameters Parameters;
 		typedef boost::shared_ptr<const Parameters> shared_parameters ;
 

tests/testGaussianFactorGraph.cpp

@@ -339,21 +339,21 @@ TEST( GaussianFactorGraph, eliminateAll )
 //	CHECK(assert_equal(expected,actual,tol));
 //}
 
-/* ************************************************************************* */
+///* ************************************************************************* */
-TEST( GaussianFactorGraph, add_priors )
+//TEST( GaussianFactorGraph, add_priors )
-{
+//{
-  Ordering ordering; ordering += "l1","x1","x2";
+//  Ordering ordering; ordering += "l1","x1","x2";
-  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-  GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
+//  GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
-  GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
+//  GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
-  Matrix A = eye(2);
+//  Matrix A = eye(2);
-  Vector b = zero(2);
+//  Vector b = zero(2);
-  SharedDiagonal sigma = sharedSigma(2,3.0);
+//  SharedDiagonal sigma = sharedSigma(2,3.0);
-  expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["l1"],A,b,sigma)));
+//  expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["l1"],A,b,sigma)));
-  expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x1"],A,b,sigma)));
+//  expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x1"],A,b,sigma)));
-  expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x2"],A,b,sigma)));
+//  expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x2"],A,b,sigma)));
-  CHECK(assert_equal(expected,actual));
+//  CHECK(assert_equal(expected,actual));
-}
+//}
 
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, copying )