Separated split() from the SubgraphSolver because it won't compile in other projects, rearranged headers to remove redundancy and fix template problems

gtsam/linear/Makefile.am

@@ -37,9 +37,9 @@ check_PROGRAMS += tests/testKalmanFilter
 
 # Iterative Methods
 headers += iterative-inl.h
-sources += iterative.cpp SubgraphPreconditioner.cpp
+sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp
 headers += IterativeSolver.h IterativeOptimizationParameters.h 
-headers += SubgraphSolver.h SubgraphSolver-inl.h
+headers += SubgraphSolver-inl.h
 
 # Timing tests
 noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike

gtsam/linear/SubgraphSolver-inl.h

@@ -11,17 +11,9 @@
 
 #pragma once
 
-#include <map>
-
 #include <boost/foreach.hpp>
-#include <boost/make_shared.hpp>
 
 #include <gtsam/linear/SubgraphSolver.h>
-#include <gtsam/linear/GaussianFactor.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/nonlinear/Key.h>
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/inference/EliminationTree-inl.h>
 
@@ -29,39 +21,6 @@ using namespace std;
 
 namespace gtsam {
 
-/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
-bool split(const std::map<Index, Index> &M,
-           const GaussianFactorGraph &Ab,
-           GaussianFactorGraph &Ab1,
-           GaussianFactorGraph &Ab2) {
-
-  Ab1 = GaussianFactorGraph();
-  Ab2 = GaussianFactorGraph();
-
-  for ( size_t i = 0 ; i < Ab.size() ; ++i ) {
-
-    boost::shared_ptr<GaussianFactor> factor = Ab[i] ;
-
-    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 ;
-}
-
-
 template<class GRAPH, class LINEAR, class VALUES>
 void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
 
@@ -85,7 +44,7 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
 	SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
 
 	pc_ = boost::make_shared<SubgraphPreconditioner>(
-	    Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
+			Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
 }
 
 template<class GRAPH, class LINEAR, class VALUES>
@@ -94,7 +53,7 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
 	// preconditioned conjugate gradient
 	VectorValues zeros = pc_->zero();
 	VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>
-		(*pc_, zeros, *parameters_);
+	(*pc_, zeros, *parameters_);
 
 	boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
 	*xbar = pc_->x(ybar);
@@ -103,29 +62,21 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
 
 template<class GRAPH, class LINEAR, class VALUES>
 void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
-		// generate spanning tree
+	// generate spanning tree
-		PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
+	PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
 
-		// make the ordering
+	// make the ordering
-		list<Key> keys = predecessorMap2Keys(tree_);
+	list<Key> keys = predecessorMap2Keys(tree_);
-		ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
+	ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
 
-		// build factor pairs
+	// build factor pairs
-		pairs_.clear();
+	pairs_.clear();
-		typedef pair<Key,Key> EG ;
+	typedef pair<Key,Key> EG ;
-		BOOST_FOREACH( const EG &eg, tree_ ) {
+	BOOST_FOREACH( const EG &eg, tree_ ) {
-			Symbol key1 = Symbol(eg.first),
+		Symbol key1 = Symbol(eg.first),
-				   key2 = Symbol(eg.second) ;
+				key2 = Symbol(eg.second) ;
-			pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
+		pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
-		}
 	}
-
-
-
-
-
-
-
-
-
 }
+
+} // \namespace gtsam

gtsam/linear/SubgraphSolver.cpp

@@ -0,0 +1,50 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+#include <gtsam/linear/SubgraphSolver.h>
+
+using namespace std;
+
+namespace gtsam {
+
+/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
+bool split(const std::map<Index, Index> &M,
+		const GaussianFactorGraph &Ab,
+		GaussianFactorGraph &Ab1,
+		GaussianFactorGraph &Ab2) {
+
+	Ab1 = GaussianFactorGraph();
+	Ab2 = GaussianFactorGraph();
+
+	for ( size_t i = 0 ; i < Ab.size() ; ++i ) {
+
+		boost::shared_ptr<GaussianFactor> factor = Ab[i] ;
+
+		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

gtsam/linear/SubgraphSolver.h

@@ -11,80 +11,88 @@
 
 #pragma once
 
+#include <boost/make_shared.hpp>
 
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
 
 namespace gtsam {
 
-  /**
+/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
-   * A nonlinear system solver using subgraph preconditioning conjugate gradient
+bool split(const std::map<Index, Index> &M,
-   * Concept NonLinearSolver<G,T,L> implements
+		const GaussianFactorGraph &Ab,
-   *   linearize: G * T -> L
+		GaussianFactorGraph &Ab1,
-   *   solve : L -> VectorValues
+		GaussianFactorGraph &Ab2);
-   */
-	template<class GRAPH, class LINEAR, class VALUES>
-	class SubgraphSolver : public IterativeSolver {
 
-	private:
+/**
-		typedef typename VALUES::Key Key;
+ * A nonlinear system solver using subgraph preconditioning conjugate gradient
-		typedef typename GRAPH::Pose Pose;
+ * Concept NonLinearSolver<G,T,L> implements
-		typedef typename GRAPH::Constraint Constraint;
+ *   linearize: G * T -> L
+ *   solve : L -> VectorValues
+ */
+template<class GRAPH, class LINEAR, class VALUES>
+class SubgraphSolver : public IterativeSolver {
 
-		typedef boost::shared_ptr<const SubgraphSolver> shared_ptr ;
+private:
-		typedef boost::shared_ptr<Ordering> shared_ordering ;
+	typedef typename VALUES::Key Key;
-		typedef boost::shared_ptr<GRAPH> shared_graph ;
+	typedef typename GRAPH::Pose Pose;
-		typedef boost::shared_ptr<LINEAR> shared_linear ;
+	typedef typename GRAPH::Constraint Constraint;
-		typedef boost::shared_ptr<VALUES> shared_values ;
-		typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
-		typedef std::map<Index,Index> mapPairIndex ;
 
-		/* the ordering derived from the spanning tree */
+	typedef boost::shared_ptr<const SubgraphSolver> shared_ptr ;
-		shared_ordering ordering_;
+	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 ;
 
-		/* the indice of two vertices in the gaussian factor graph */
+	/* the ordering derived from the spanning tree */
-		mapPairIndex pairs_;
+	shared_ordering ordering_;
 
-		/* preconditioner */
+	/* the indice of two vertices in the gaussian factor graph */
-		shared_preconditioner pc_;
+	mapPairIndex pairs_;
 
-		/* flag for direct solver - either QR or LDL */
+	/* preconditioner */
-		bool useQR_;
+	shared_preconditioner pc_;
 
-	public:
+	/* flag for direct solver - either QR or LDL */
+	bool useQR_;
 
-		SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
+public:
-			IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
 
-		SubgraphSolver(const LINEAR& GFG) {
+	SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
-			std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
+		IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
-			throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
-		}
 
-		SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure, bool useQR = false) {
+	SubgraphSolver(const LINEAR& GFG) {
-			std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
+		std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
-			throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported");
+		throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
-		}
+	}
 
-		SubgraphSolver(const SubgraphSolver& solver) :
+	SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure, bool useQR = false) {
-			IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {}
+		std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
+		throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported");
+	}
 
-		SubgraphSolver(shared_ordering ordering,
+	SubgraphSolver(const SubgraphSolver& solver) :
-				mapPairIndex pairs,
+		IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {}
-				shared_preconditioner pc,
-				sharedParameters parameters = boost::make_shared<Parameters>(),
-				bool useQR = true) :
-					IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
 
-		void replaceFactors(const typename LINEAR::shared_ptr &graph);
+	SubgraphSolver(shared_ordering ordering,
-		VectorValues::shared_ptr optimize() const ;
+			mapPairIndex pairs,
-		shared_ordering ordering() const { return ordering_; }
+			shared_preconditioner pc,
+			sharedParameters parameters = boost::make_shared<Parameters>(),
+			bool useQR = true) :
+				IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
 
-	protected:
+	void replaceFactors(const typename LINEAR::shared_ptr &graph);
-		void initialize(const GRAPH& G, const VALUES& theta0);
+	VectorValues::shared_ptr optimize() const ;
+	shared_ordering ordering() const { return ordering_; }
 
-	private:
+protected:
-	  	SubgraphSolver():IterativeSolver(){}
+	void initialize(const GRAPH& G, const VALUES& theta0);
-	};
+
+private:
+	SubgraphSolver():IterativeSolver(){}
+};
 
 } // nsamespace gtsam

gtsam/nonlinear/NonlinearOptimization-inl.h

@@ -20,9 +20,10 @@
 
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/nonlinear/NonlinearOptimization.h>
+
 #include <gtsam/linear/SubgraphSolver-inl.h>
 #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
-#include <gtsam/nonlinear/NonlinearOptimization.h>
 
 using namespace std;
 

gtsam/nonlinear/NonlinearOptimizationParameters.h

@@ -20,6 +20,7 @@
 
 #include <boost/shared_ptr.hpp>
 #include <boost/make_shared.hpp>
+#include <boost/serialization/nvp.hpp>
 
 namespace gtsam {