diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index ca2b93f04..da4d725da 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -146,60 +146,6 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
 	return fg;
 }
 
-bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const {
-
-	//typedef sharedFactor F ;
-
-	Ab1 = GaussianFactorGraph();
-	Ab2 = GaussianFactorGraph();
-
-	BOOST_FOREACH(const sharedFactor& factor, factors_) {
-
-		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 ;
-}
-
-VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
-	std::vector<size_t> dimensions(size()) ;
-	Index i = 0 ;
-	BOOST_FOREACH( const sharedFactor& factor, factors_) {
-		dimensions[i] = factor->numberOfRows() ;
-		i++;
-	}
-
-	return VectorValues(dimensions) ;
-}
-
-
-bool GaussianFactorGraph::getDiagonalOfHessian(VectorValues &values) const {
-
-	values.makeZero() ;
-
-	BOOST_FOREACH( const sharedFactor& factor, factors_) {
-		for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
-			Vector v = columnNormSquare(factor->getA(j));
-			values[*j] += v;
-		}
-	}
-	return true ;
-}
-
 void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const {
 
 	getb(r) ;
@@ -231,6 +177,17 @@ void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues
 	}
 }
 
+VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
+    std::vector<size_t> dimensions(size()) ;
+    Index i = 0 ;
+    BOOST_FOREACH( const sharedFactor& factor, factors_) {
+        dimensions[i] = factor->numberOfRows() ;
+        i++;
+    }
+
+    return VectorValues(dimensions) ;
+}
+
 void GaussianFactorGraph::getb(VectorValues &b) const {
 	Index i = 0 ;
 	BOOST_FOREACH( const sharedFactor& factor, factors_) {
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index c0d0dbcc0..b4b36a7ba 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -151,23 +151,18 @@ namespace gtsam {
      */
     void combine(const GaussianFactorGraph &lfg);
 
-	/**
-	 * 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 ;
-
-	// allocate a vectorvalues of b's structure
-	VectorValues allocateVectorValuesb() const ;
-
-	/* get the diagonal of A^ A, used to build jacobi preconditioner */
-	bool getDiagonalOfHessian(VectorValues &values) const ;
-
+    // matrix-vector operations
 	void residual(const VectorValues &x, VectorValues &r) const ;
 	void multiply(const VectorValues &x, VectorValues &r) const ;
 	void transposeMultiply(const VectorValues &r, VectorValues &x) const ;
+
+	// get b
 	void getb(VectorValues &b) const ;
 	VectorValues getb() const ;
+
+    // allocate a vectorvalues of b's structure
+    VectorValues allocateVectorValuesb() const ;
+
   };
 
 } // namespace gtsam
diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h
index 8d32f5c36..a3dbebbb8 100644
--- a/gtsam/linear/SubgraphSolver-inl.h
+++ b/gtsam/linear/SubgraphSolver-inl.h
@@ -11,11 +11,14 @@
 
 #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/nonlinear/Key.h>
 #include <gtsam/linear/iterative-inl.h>
@@ -26,6 +29,40 @@ 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) {
 
@@ -33,7 +70,7 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
 				  Ab2 = boost::make_shared<LINEAR>();
 
 	if (parameters_->verbosity()) cout << "split the graph ...";
-	graph->split(pairs_, *Ab1, *Ab2) ;
+	split(pairs_, *graph, *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