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>
@@ -39,15 +39,10 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template <class FG>
-	JunctionTree<FG>::JunctionTree(const FG& fg) {
+  void JunctionTree<FG>::construct(const FG& fg, const VariableIndex& variableIndex) {
     tic("JT 1  constructor");
-		// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph
-		// -> SymbolicBayesNet -> SymbolicBayesTree
     tic("JT 1.1  symbolic elimination");
-		SymbolicBayesNet::shared_ptr sbn = SymbolicSequentialSolver(fg).eliminate();
+    SymbolicBayesNet::shared_ptr sbn = EliminationTree<IndexFactor>::Create(fg, variableIndex)->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);
@@ -60,6 +55,18 @@ namespace gtsam {
     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(

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 )