Fix the subgraph preconditioner solver

linear/SubgraphPreconditioner.cpp

@@ -41,30 +41,31 @@ namespace gtsam {
 		return x;
 	}
 
+    SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const {
+    	SubgraphPreconditioner result = *this ;
+    	result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ;
+    	return result ;
+    }
+
 	/* ************************************************************************* */
 	double SubgraphPreconditioner::error(const VectorValues& y) const {
 
 //		Errors e;
-//
 //		// Use BayesNet order to add y contributions in order
 //		BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
 //			const Symbol& j = cg->key();
 //			e.push_back(y[j]); // append y
 //		}
-//
 //		// Add A2 contribution
 //		VectorValues x = this->x(y);
 //		Errors e2 = Ab2_->errors(x);
 //		e.splice(e.end(), e2);
-//
 //		return 0.5 * dot(e, e);
 
-		Errors e(y);
 
-		// Add A2 contribution
+		Errors e(y);
 		VectorValues x = this->x(y);
 		Errors e2 = Ab2_->errors(x);
-
 		return 0.5 * (dot(e, e) + dot(e2,e2));
 	}
 
@@ -107,21 +108,19 @@ namespace gtsam {
 	void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
 
 //		Errors::iterator ei = e.begin();
-//
 //		// Use BayesNet order to add y contributions in order
 //		BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
 //			const Symbol& j = cg->key();
 //			*ei = y[j]; // append y
 //			ei++;
 //		}
-//
 //		// Add A2 contribution
 //		VectorValues x = y; // TODO avoid ?
 //		gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y
 //		Ab2_->multiplyInPlace(x,ei); // use iterator version
 
 		Errors::iterator ei = e.begin();
-		for ( int i = 0 ; i < y.size() ; ++i, ++ei ) {
+		for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
 			*ei = y[i];
 		}
 
@@ -152,7 +151,7 @@ namespace gtsam {
 
 		Errors::const_iterator it = e.begin();
 		VectorValues y = zero();
-		for ( int i = 0 ; i < y.size() ; ++i, ++it )
+		for ( Index i = 0 ; i < y.size() ; ++i, ++it )
 			y[i] = *it ;
 		transposeMultiplyAdd2(1.0,it,e.end(),y);
 		return y;
@@ -170,12 +169,11 @@ namespace gtsam {
 //			const Vector& ej = *(it++);
 //			axpy(alpha,ej,y[j]);
 //		}
-//
 //		// get A2 part
 //		transposeMultiplyAdd2(alpha,it,e.end(),y);
 
 		Errors::const_iterator it = e.begin();
-		for ( int i = 0 ; i < y.size() ; ++i, ++it ) {
+		for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
 			const Vector& ei = *it;
 			axpy(alpha,ei,y[i]) ;
 		}

linear/SubgraphPreconditioner.h

@@ -63,6 +63,12 @@ namespace gtsam {
 //		Vector b2() const { return Ab2_->rhsVector(); }
 //		VectorValues assembleValues(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleValues(v, ordering); }
 
+	    /**
+	     * Add zero-mean i.i.d. Gaussian prior terms to each variable
+	     * @param sigma Standard deviation of Gaussian
+	     */
+	    SubgraphPreconditioner add_priors(double sigma) const;
+
 		/* x = xbar + inv(R1)*y */
 		VectorValues x(const VectorValues& y) const;
 

linear/SubgraphSolver-inl.h

@@ -21,9 +21,8 @@
 
 #include <boost/tuple/tuple.hpp>
 #include <gtsam/linear/SubgraphSolver.h>
-
-#include <gtsam/inference/graph-inl.h>
 #include <gtsam/linear/iterative-inl.h>
+#include <gtsam/inference/graph-inl.h>
 #include <gtsam/inference/FactorGraph-inl.h>
 
 using namespace std;
@@ -41,36 +40,37 @@ namespace gtsam {
 	void SubgraphSolver<Graph, Values>::initialize(const Graph& G, const Values& theta0) {
 
 		// generate spanning tree
-		PredecessorMap<Key> tree = G.template findMinimumSpanningTree<Key, Constraint>();
+		PredecessorMap<Key> tree = gtsam::findMinimumSpanningTree<Graph, Key, Constraint>(G);
-		list<Key> keys = predecessorMap2Keys(tree);
 
 		// split the graph
 		if (verbose_) cout << "generating spanning tree and split the graph ...";
-		G.template split<Key, Constraint>(tree, T_, C_);
+		// G.template split<Key, Constraint>(tree, T_, C_);
+		gtsam::split<Graph,Key,Constraint>(G, tree, T_, C_) ;
 		if (verbose_) cout << T_.size() << " and " << C_.size() << " factors" << endl;
 
 		// make the ordering
-		list<Symbol> symbols;
+		list<Key> keys = predecessorMap2Keys(tree);
-		symbols.resize(keys.size());
+		ordering_ = boost::shared_ptr<Ordering>(new Ordering(list<Symbol>(keys.begin(), keys.end())));
-		std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol<Key>);
+
-		ordering_ = boost::shared_ptr<Ordering>(new Ordering(symbols));
+		// Add a HardConstraint to the root, otherwise the root will be singular
+		Key root = keys.back();
+		T_.addHardConstraint(root, theta0[root]);
 
 		// compose the approximate solution
-		Key root = keys.back();
 		theta_bar_ = composePoses<Graph, Constraint, Pose, Values> (T_, tree, theta0[root]);
 	}
 
 	/* ************************************************************************* */
 	template<class Graph, class Values>
 	boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<Graph, Values>::linearize(const Graph& G, const Values& theta_bar) const {
-		SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar);
+		SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar, *ordering_);
-		SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar);
+		SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_);
 #ifdef TIMING
 		SubgraphPreconditioner::sharedBayesNet Rc1;
 		SubgraphPreconditioner::sharedValues xbar;
 #else
 		GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
-		SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_);
+		SubgraphPreconditioner::sharedBayesNet Rc1 = Inference::Eliminate(sacrificialAb1) ;
 		SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
 #endif
 		// TODO: there does not seem to be a good reason to have Ab1_

linear/SubgraphSolver.h

@@ -19,8 +19,8 @@
 
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/inference/Ordering.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
 
@@ -67,6 +67,7 @@ namespace gtsam {
 		 */
 		boost::shared_ptr<SubgraphPreconditioner> linearize(const Graph& G, const Values& theta_bar) const;
 
+
 		/**
 		 * solve for the optimal displacement in the tangent space, and then solve
 		 * the resulted linear system
@@ -76,6 +77,12 @@ namespace gtsam {
 		boost::shared_ptr<SubgraphSolver> prepareLinear(const SubgraphPreconditioner& fg) const {
 			return boost::shared_ptr<SubgraphSolver>(new SubgraphSolver(*this));
 		}
+
+
+	  	/** expmap the Values given the stored Ordering */
+	  	Values expmap(const Values& config, const VectorValues& delta) const {
+	  	  return config.expmap(delta, *ordering_);
+	  	}
 	};
 
 	template<class Graph, class Values> const size_t SubgraphSolver<Graph,Values>::maxIterations_;