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) {
 
@@ -118,14 +77,6 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUE
 				key2 = Symbol(eg.second) ;
 		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,22 +11,30 @@
 
 #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 */
+bool split(const std::map<Index, Index> &M,
+		const GaussianFactorGraph &Ab,
+		GaussianFactorGraph &Ab1,
+		GaussianFactorGraph &Ab2);
+
+/**
  * A nonlinear system solver using subgraph preconditioning conjugate gradient
  * Concept NonLinearSolver<G,T,L> implements
  *   linearize: G * T -> L
  *   solve : L -> VectorValues
  */
-	template<class GRAPH, class LINEAR, class VALUES>
+template<class GRAPH, class LINEAR, class VALUES>
-	class SubgraphSolver : public IterativeSolver {
+class SubgraphSolver : public IterativeSolver {
 
-	private:
+private:
 	typedef typename VALUES::Key Key;
 	typedef typename GRAPH::Pose Pose;
 	typedef typename GRAPH::Constraint Constraint;
@@ -51,7 +59,7 @@ namespace gtsam {
 	/* flag for direct solver - either QR or LDL */
 	bool useQR_;
 
-	public:
+public:
 
 	SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
 		IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
@@ -80,11 +88,11 @@ namespace gtsam {
 	VectorValues::shared_ptr optimize() const ;
 	shared_ordering ordering() const { return ordering_; }
 
-	protected:
+protected:
 	void initialize(const GRAPH& G, const VALUES& theta0);
 
-	private:
+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 {