adapt spcg to new optimization interface

examples/Makefile.am

@@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy			# Solves SLAM example from tutorial
 noinst_PROGRAMS += PlanarSLAMSelfContained_advanced	# Solves SLAM example from tutorial with all typedefs in the script
 noinst_PROGRAMS += Pose2SLAMExample_easy 			# Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
 noinst_PROGRAMS += Pose2SLAMExample_advanced		# Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
-#noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
+noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
-#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
+noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
 #SUBDIRS = vSLAMexample # does not compile....
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs

examples/Pose2SLAMwSPCG_advanced.cpp

@@ -25,9 +25,16 @@ using namespace std;
 using namespace gtsam;
 using namespace pose2SLAM;
 
-typedef boost::shared_ptr<Graph> sharedGraph;
+typedef boost::shared_ptr<Graph> sharedGraph ;
-typedef boost::shared_ptr<Values> sharedValue;
+typedef boost::shared_ptr<Values> sharedValue ;
-typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
+//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
+
+
+typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
+
+typedef boost::shared_ptr<Solver> sharedSolver ;
+
+typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
 
 sharedGraph graph;
 sharedValue initial;
@@ -44,8 +51,8 @@ int main(void) {
 	graph->print("full graph") ;
 	initial->print("initial estimate") ;
 
-	SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*graph, *initial)) ;
+	sharedSolver solver(new Solver(*graph, *initial)) ;
-	SPCGOptimizer optimizer(graph, initial, solver) ;
+	SPCGOptimizer optimizer(graph, initial, solver->ordering(), solver) ;
 
 	cout << "before optimization, sum of error is " << optimizer.error() << endl;
 	NonlinearOptimizationParameters parameter;

linear/GaussianFactorGraph.cpp

@@ -163,4 +163,34 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector<s
 	return result;
 }
 
+bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const {
+
+	typedef sharedFactor F ;
+
+	Ab1 = GaussianFactorGraph();
+	Ab2 = GaussianFactorGraph();
+
+	BOOST_FOREACH(const F& factor, *this) {
+
+		if (factor->keys().size() > 2)
+			throw(invalid_argument("split: only support factors with at most two keys"));
+		if (factor->keys().size() == 1) {
+			Ab1.push_back(factor);
+			Ab2.push_back(factor);
+			continue;
+		}
+		Index key1 = factor->keys_[0];
+		Index key2 = factor->keys_[1];
+
+		if ((M.find(key1) != M.end() && M.find(key1)->second == key2) ||
+			(M.find(key2) != M.end() && M.find(key2)->second == key1))
+			Ab1.push_back(factor);
+		else
+			Ab2.push_back(factor);
+	}
+
+	return true ;
+}
+
+
 } // namespace gtsam

linear/GaussianFactorGraph.h

@@ -152,6 +152,13 @@ namespace gtsam {
      */
     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.
+	 */
+	bool split(const std::map<Index, Index> &M, GaussianFactorGraph &A1, GaussianFactorGraph &A2) const ;
+
   };
 
 } // namespace gtsam

linear/GaussianMultifrontalSolver.cpp

@@ -59,7 +59,7 @@ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const {
 std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalStandard(Index j) const {
 	GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
 	Matrix R = conditional->get_R();
-	return make_pair(conditional->get_d(), inverse(trans(R)*R));
+	return make_pair(conditional->get_d(), inverse(prod(trans(R),R)));
 }
 
 }

linear/GaussianSequentialSolver.cpp

@@ -75,7 +75,7 @@ GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const {
 std::pair<Vector, Matrix> GaussianSequentialSolver::marginalStandard(Index j) const {
 	GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
 	Matrix R = conditional->get_R();
-	return make_pair(conditional->get_d(), inverse(trans(R)*R));
+	return make_pair(conditional->get_d(), inverse(prod(trans(R),R)));
 }
 
 

linear/SubgraphPreconditioner.cpp

@@ -23,8 +23,8 @@ using namespace std;
 namespace gtsam {
 
 	/* ************************************************************************* */
-	SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2,
+	SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2,
-			sharedBayesNet& Rc1, sharedValues& xbar) :
+			const sharedBayesNet& Rc1, const sharedValues& xbar) :
 		Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) {
 	}
 
@@ -50,8 +50,6 @@ namespace gtsam {
 	/* ************************************************************************* */
 	double SubgraphPreconditioner::error(const VectorValues& y) const {
 
-
-
 		Errors e(y);
 		VectorValues x = this->x(y);
 		Errors e2 = Ab2_->errors(x);

linear/SubgraphPreconditioner.h

@@ -46,6 +46,7 @@ namespace gtsam {
 
 	public:
 
+		SubgraphPreconditioner();
 		/**
 		 * Constructor
 		 * @param Ab1: the Graph A1*x=b1
@@ -53,7 +54,7 @@ namespace gtsam {
 		 * @param Rc1: the Bayes Net R1*x=c1
 		 * @param xbar: the solution to R1*x=c1
 		 */
-		SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1,	sharedValues& xbar);
+		SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1,	const sharedValues& xbar);
 
 
 	    /**

linear/SubgraphSolver-inl.h

@@ -19,75 +19,8 @@
 
 #pragma once
 
-#include <boost/tuple/tuple.hpp>
 #include <gtsam/linear/SubgraphSolver.h>
-#include <gtsam/linear/iterative-inl.h>
-#include <gtsam/inference/graph-inl.h>
-#include <gtsam/inference/FactorGraph-inl.h>
-#include <gtsam/inference/EliminationTree-inl.h>
 
 using namespace std;
 
-namespace gtsam {
+namespace gtsam {}
-
-	/* ************************************************************************* */
-	template<class GRAPH, class VALUES>
-	SubgraphSolver<GRAPH, VALUES>::SubgraphSolver(const GRAPH& G, const VALUES& theta0) {
-		initialize(G,theta0);
-	}
-
-	/* ************************************************************************* */
-	template<class GRAPH, class VALUES>
-	void SubgraphSolver<GRAPH, VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
-
-		// generate spanning tree
-		PredecessorMap<Key> tree = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
-
-		// split the graph
-		if (verbose_) cout << "generating spanning tree and split the graph ...";
-		gtsam::split<GRAPH,Key,Constraint>(G, tree, T_, C_) ;
-		if (verbose_) cout << ",with " << T_.size() << " and " << C_.size() << " factors" << endl;
-
-		// make the ordering
-		list<Key> keys = predecessorMap2Keys(tree);
-		ordering_ = boost::shared_ptr<Ordering>(new Ordering(list<Symbol>(keys.begin(), keys.end())));
-
-		// 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
-		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, *ordering_);
-		SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_);
-#ifdef TIMING
-		SubgraphPreconditioner::sharedBayesNet Rc1;
-		SubgraphPreconditioner::sharedValues xbar;
-#else
-		GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
-		SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
-		SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
-#endif
-		// TODO: there does not seem to be a good reason to have Ab1_
-		// It seems only be used to provide an ordering for creating sparse matrices
-		return boost::shared_ptr<SubgraphPreconditioner>(new SubgraphPreconditioner(Ab1, Ab2, Rc1, xbar));
-	}
-
-	/* ************************************************************************* */
-	template<class GRAPH, class VALUES>
-	VectorValues SubgraphSolver<GRAPH, VALUES>::optimize(SubgraphPreconditioner& system) const {
-		VectorValues zeros = system.zero();
-
-		// Solve the subgraph PCG
-		VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
-				Errors> (system, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_);
-		VectorValues xbar = system.x(ybar);
-		return xbar;
-	}
-
-}

linear/SubgraphSolver.h

@@ -9,17 +9,19 @@
 
  * -------------------------------------------------------------------------- */
 
-/*
- * SubgraphSolver.h
- * Created on: Dec 31, 2009
- * @author: Frank Dellaert
- */
-
 #pragma once
 
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+
+#include <gtsam/inference/EliminationTree-inl.h>
+#include <gtsam/linear/iterative-inl.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/nonlinear/Key.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
@@ -30,13 +32,21 @@ namespace gtsam {
    *   linearize: G * T -> L
    *   solve : L -> VectorValues
    */
-	template<class GRAPH, class VALUES>
+	template<class GRAPH, class LINEAR, class VALUES>
 	class SubgraphSolver {
 
 	private:
 		typedef typename VALUES::Key Key;
-		typedef typename GRAPH::Constraint Constraint;
 		typedef typename GRAPH::Pose Pose;
+		typedef typename GRAPH::Constraint Constraint;
+
+		typedef boost::shared_ptr<const SubgraphSolver> shared_ptr ;
+		typedef boost::shared_ptr<Ordering> shared_ordering ;
+		typedef boost::shared_ptr<GRAPH> shared_graph ;
+		typedef boost::shared_ptr<LINEAR> shared_linear ;
+		typedef boost::shared_ptr<VALUES> shared_values ;
+		typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
+		typedef std::map<Index,Index> mapPairIndex ;
 
 		// TODO not hardcode
 		static const size_t maxIterations_=100;
@@ -44,50 +54,95 @@ namespace gtsam {
 		static const bool verbose_=true;
 
 		/* the ordering derived from the spanning tree */
-		boost::shared_ptr<Ordering> ordering_;
+		shared_ordering ordering_;
 
-		/* the solution computed from the first subgraph */
+		/* the indice of two vertices in the gaussian factor graph */
-		boost::shared_ptr<VALUES> theta_bar_;
+		mapPairIndex pairs_;
 
-		GRAPH T_, C_;
+		/* preconditioner */
+		shared_preconditioner pc_;
 
 	public:
-		SubgraphSolver() {}
+	  	SubgraphSolver(){}
 
-		SubgraphSolver(const GRAPH& G, const VALUES& theta0);
+	  	SubgraphSolver(const LINEAR &GFG) {
-
+			throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
-		void initialize(const GRAPH& G, const VALUES& theta0);
-
-		boost::shared_ptr<Ordering> ordering() const { return ordering_; }
-
-		boost::shared_ptr<VALUES> theta_bar() const { return theta_bar_; }
-
-		/**
-		 * linearize the non-linear graph around the current config and build the subgraph preconditioner systme
-		 */
-		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
-		 */
-		VectorValues optimize(SubgraphPreconditioner& system) const;
-
-		boost::shared_ptr<SubgraphSolver> prepareLinear(const SubgraphPreconditioner& fg) const {
-			return boost::shared_ptr<SubgraphSolver>(new SubgraphSolver(*this));
 	  	}
 
+	  	SubgraphSolver(const SubgraphSolver& solver) :
+	  		ordering_(solver.ordering_), pairs_(solver.pairs_),	pc_(solver.pc_){}
 
-	  	/** expmap the Values given the stored Ordering */
+	  	SubgraphSolver(shared_ordering ordering,
-	  	VALUES expmap(const VALUES& config, const VectorValues& delta) const {
+	  				   mapPairIndex pairs,
-	  	  return config.expmap(delta, *ordering_);
+	  				   shared_preconditioner pc) :
+	  		ordering_(ordering), pairs_(pairs), pc_(pc) {}
+
+
+		SubgraphSolver(const GRAPH& G, const VALUES& theta0) { initialize(G,theta0); }
+
+		shared_ptr update(const LINEAR &graph) const {
+
+			shared_linear Ab1 = boost::make_shared<LINEAR>(),
+						  Ab2 = boost::make_shared<LINEAR>();
+
+			if (verbose_) cout << "split the graph ...";
+			graph.split(pairs_, *Ab1, *Ab2) ;
+			if (verbose_) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
+
+			//		// Add a HardConstra	int to the root, otherwise the root will be singular
+			//		Key root = keys.back();
+			//		T_.addHardConstraint(root, theta0[root]);
+			//
+			//		// compose the approximate solution
+			//		theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
+
+			LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
+			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);
+			return boost::make_shared<SubgraphSolver>(ordering_, pairs_, pc) ;
 		}
+
+		 VectorValues::shared_ptr optimize() const {
+
+			// preconditioned conjugate gradient
+			VectorValues zeros = pc_->zero();
+			VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
+					Errors> (*pc_, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_);
+
+			boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
+			*xbar = pc_->x(ybar);
+			return xbar;
+		}
+
+		shared_ordering ordering() const { return ordering_; }
+
+	protected:
+
+		void initialize(const GRAPH& G, const VALUES& theta0) {
+			// generate spanning tree
+			PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
+
+			// make the ordering
+			list<Key> keys = predecessorMap2Keys(tree_);
+			ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
+
+			// build factor pairs
+			pairs_.clear();
+			typedef pair<Key,Key> EG ;
+			BOOST_FOREACH( const EG &eg, tree_ ) {
+				Symbol key1 = Symbol(eg.first),
+					   key2 = Symbol(eg.second) ;
+				pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
+			}
+		}
+
 	};
 
-	template<class GRAPH, class VALUES> const size_t SubgraphSolver<GRAPH,VALUES>::maxIterations_;
+	template<class GRAPH, class LINEAR, class VALUES> const size_t SubgraphSolver<GRAPH, LINEAR, VALUES>::maxIterations_;
-	template<class GRAPH, class VALUES> const bool SubgraphSolver<GRAPH,VALUES>::verbose_;
+	template<class GRAPH, class LINEAR, class VALUES> const bool SubgraphSolver<GRAPH, LINEAR, VALUES>::verbose_;
-	template<class GRAPH, class VALUES> const double SubgraphSolver<GRAPH,VALUES>::epsilon_;
+	template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_;
-	template<class GRAPH, class VALUES> const double SubgraphSolver<GRAPH,VALUES>::epsilon_abs_;
+	template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_abs_;
 
 } // nsamespace gtsam

nonlinear/NonlinearOptimization-inl.h

@@ -67,24 +67,27 @@ namespace gtsam {
 		return *result.values();
 	}
 
-//	/**
+	/**
-//	 * The sparse preconditioned conjucate gradient solver
+	 * The sparse preconditioned conjucate gradient solver
-//	 */
+	 */
-//	template<class G, class T>
+	template<class G, class T>
-//	T	optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
+	T	optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
-//
+
-//		// initial optimization state is the same in both cases tested
+		// initial optimization state is the same in both cases tested
-//		typedef NonlinearOptimizer<G, T, SubgraphPreconditioner, SubgraphSolver<G,T> > SPCGOptimizer;
+		typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver;
-//		typename SPCGOptimizer::shared_solver solver(new SubgraphSolver<G,T>(graph, initialEstimate));
+		typedef boost::shared_ptr<Solver> shared_Solver;
-//		SPCGOptimizer optimizer(
+		typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
-//				boost::make_shared<const G>(graph),
+		shared_Solver solver = boost::make_shared<Solver>(graph, initialEstimate);
-//				boost::make_shared<const T>(initialEstimate),
+		SPCGOptimizer optimizer(
-//				solver);
+				boost::make_shared<const G>(graph),
-//
+				boost::make_shared<const T>(initialEstimate),
-//		// Levenberg-Marquardt
+				solver->ordering(),
-//		SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
+				solver);
-//		return *result.values();
+
-//	}
+		// Levenberg-Marquardt
+		SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
+		return *result.values();
+	}
 
 	/**
 	 * optimization that returns the values

nonlinear/NonlinearOptimizer-inl.h

@@ -85,6 +85,24 @@ namespace gtsam {
 				"NonlinearOptimizer constructor: ordering = NULL");
 	}
 
+	template<class G, class C, class L, class S, class W>
+	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(
+			shared_graph graph,	shared_values values, shared_ordering ordering, shared_solver solver, const double lambda):
+			graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), solver_(solver),
+			lambda_(lambda), dimensions_(new vector<size_t>(values->dims(*ordering))) {
+		if (!graph) throw std::invalid_argument(
+				"NonlinearOptimizer constructor: graph = NULL");
+		if (!values) throw std::invalid_argument(
+				"NonlinearOptimizer constructor: values = NULL");
+		if (!ordering) throw std::invalid_argument(
+				"NonlinearOptimizer constructor: ordering = NULL");
+		if (!solver) throw std::invalid_argument(
+				"NonlinearOptimizer constructor: solver = NULL");
+	}
+
+
+
+
 	/* ************************************************************************* */
 	// linearize and optimize
 	/* ************************************************************************* */

nonlinear/NonlinearOptimizer.h

@@ -130,6 +130,13 @@ namespace gtsam {
 		NonlinearOptimizer(shared_graph graph, shared_values values, shared_ordering ordering,
 				const double lambda = 1e-5);
 
+		NonlinearOptimizer(shared_graph graph,
+						   shared_values values,
+						   shared_ordering ordering,
+						   shared_solver solver,
+						   const double lambda = 1e-5);
+
+
 		/**
 		 * Copy constructor
 		 */