add a simple conjugate gradient solver with a structure to encapsulate all parameters for iterative methods

linear/ConjugateGradientSolver-inl.h

@@ -0,0 +1,10 @@
+/*
+ * ConjugateGradientSolver-inl.h
+ *
+ *  Created on: Oct 24, 2010
+ *      Author: Yong-Dian Jian
+ */
+
+#pragma once
+
+#include <gtsam/ConjugateGradientSolver.h>

linear/ConjugateGradientSolver.h

@@ -0,0 +1,58 @@
+/*
+ * ConjugateGradientSolver.h
+ *
+ *  Created on: Oct 21, 2010
+ *      Author: Yong-Dian Jian
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include <gtsam/linear/IterativeSolver.h>
+#include <gtsam/linear/iterative-inl.h>
+#include <gtsam/nonlinear/Ordering.h>
+
+namespace gtsam {
+
+	// GaussianFactorGraph, Values
+ 	template<class GRAPH, class LINEAR, class VALUES>
+	class ConjugateGradientSolver : public IterativeSolver {
+
+	protected:
+		const LINEAR *ptr_;
+
+		typedef boost::shared_ptr<VectorValues> sharedVectorValues ;
+		sharedVectorValues zeros_;
+
+	public:
+
+		typedef boost::shared_ptr<const ConjugateGradientSolver> shared_ptr ;
+
+		ConjugateGradientSolver(const GRAPH &graph, const VALUES &initial, const Ordering &ordering, const sharedParameters parameters):
+			IterativeSolver(parameters), ptr_(0), zeros_(boost::make_shared<VectorValues>(initial.zero(ordering))) {}
+
+		ConjugateGradientSolver(const LINEAR &GFG) {
+			throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
+	  	}
+
+		ConjugateGradientSolver(const ConjugateGradientSolver& solver) : IterativeSolver(solver), ptr_(solver.ptr_), zeros_(solver.zeros_){}
+		ConjugateGradientSolver(const sharedParameters parameters, const LINEAR *ptr, const sharedVectorValues zeros):
+			IterativeSolver(parameters), ptr_(ptr), zeros_(zeros) {}
+
+		shared_ptr update(const LINEAR &graph) const {
+			return boost::make_shared<ConjugateGradientSolver>(parameters_, &graph, zeros_) ;
+		}
+
+		VectorValues::shared_ptr optimize() const {
+			//boost::shared_ptr<VectorValues> zeros (ptr_->allocateVectorVavlues());
+			VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, parameters_);
+			return boost::make_shared<VectorValues>(x) ;
+		}
+
+	private:
+		ConjugateGradientSolver():IterativeSolver(),ptr_(0){}
+
+	};
+
+} // nsamespace gtsam

linear/GaussianFactorGraph.cpp

@@ -16,7 +16,10 @@
  * @author  Christian Potthast
  */
 
+#include <vector>
+
 #include <boost/foreach.hpp>
+#include <boost/make_shared.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/numeric/ublas/lu.hpp>
 #include <boost/numeric/ublas/io.hpp>
@@ -165,12 +168,12 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector<s
 
 bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const {
 
-	typedef sharedFactor F ;
+	//typedef sharedFactor F ;
 
 	Ab1 = GaussianFactorGraph();
 	Ab2 = GaussianFactorGraph();
 
-	BOOST_FOREACH(const F& factor, *this) {
+	BOOST_FOREACH(const sharedFactor& factor, factors_) {
 
 		if (factor->keys().size() > 2)
 			throw(invalid_argument("split: only support factors with at most two keys"));
@@ -192,5 +195,18 @@ bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorG
 	return true ;
 }
 
+boost::shared_ptr<VectorValues> GaussianFactorGraph::allocateVectorVavlues() const {
+	std::vector<size_t> dimensions(size()) ;
+	BOOST_FOREACH( const sharedFactor& factor, factors_) {
+		Index i = 0 ;
+		BOOST_FOREACH( const Index& idx, factor->keys_) {
+			dimensions[idx] = factor->Ab_(i).size2() ;
+			i++;
+		}
+	}
+
+	return boost::make_shared<VectorValues>(dimensions) ;
+}
+
 
 } // namespace gtsam

linear/GaussianFactorGraph.h

@@ -158,6 +158,7 @@ namespace gtsam {
 	 * 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 ;
+	boost::shared_ptr<VectorValues> allocateVectorVavlues() const ;
 
   };
 

linear/IterativeOptimizationParameters.h

@@ -0,0 +1,44 @@
+/*
+ * IterativeOptimizationParameters.h
+ *
+ *  Created on: Oct 22, 2010
+ *      Author: Yong-Dian Jian
+ */
+
+#pragma once
+
+namespace gtsam {
+
+	// a container for all related parameters
+	struct IterativeOptimizationParameters {
+		typedef enum {
+			SILENT,
+			ERROR,
+		} verbosityLevel;
+
+		IterativeOptimizationParameters():
+			maxIterations_(100),
+			reset_(101),
+			epsilon_(1e-5),epsilon_abs_(1e-5),verbosity_(SILENT) {}
+
+		IterativeOptimizationParameters
+		(int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=SILENT, int reset=-1):
+		maxIterations_(maxIterations), reset_(reset),
+		epsilon_(epsilon), epsilon_abs_(epsilon_abs), verbosity_(verbosity) {
+			if (reset_==-1) reset_ = maxIterations_ + 1 ;
+		}
+
+		int maxIterations() const { return maxIterations_; }
+		int reset() const { return reset_; }
+		double epsilon() const { return epsilon_ ;}
+		double epsilon_abs() const { return epsilon_abs_ ; }
+		verbosityLevel verbosity() const { return verbosity_ ; }
+
+	protected:
+		int maxIterations_;
+		int reset_ ; // number of iterations before reset, for cg and gmres
+		double epsilon_; // relative error
+		double epsilon_abs_; // absolute error
+		verbosityLevel verbosity_;
+	};
+}

linear/IterativeSolver.h

@@ -0,0 +1,37 @@
+/*
+ * IterativeSolver.h
+ *
+ *  Created on: Oct 24, 2010
+ *      Author: Yong-Dian Jian
+ *
+ *  Base Class for all iterative solvers of linear systems
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+
+#include <gtsam/linear/IterativeOptimizationParameters.h>
+
+namespace gtsam {
+
+class IterativeSolver {
+
+public:
+	typedef IterativeOptimizationParameters Parameters;
+	typedef boost::shared_ptr<Parameters> sharedParameters;
+
+	sharedParameters parameters_ ;
+
+	IterativeSolver():
+		parameters_(new IterativeOptimizationParameters()) {}
+
+	IterativeSolver(const IterativeSolver &solver):
+		parameters_(solver.parameters_) {}
+
+	IterativeSolver(const sharedParameters parameters):
+		parameters_(parameters) {}
+};
+
+}

linear/SubgraphSolver-inl.h

@@ -9,18 +9,87 @@
 
  * -------------------------------------------------------------------------- */
 
-/*
- * SubgraphSolver-inl.h
- *
- *   Created on: Jan 17, 2010
- *       Author: nikai
- *  Description: subgraph preconditioning conjugate gradient solver
- */
-
 #pragma once
 
+#include <boost/foreach.hpp>
+#include <boost/make_shared.hpp>
+
 #include <gtsam/linear/SubgraphSolver.h>
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/nonlinear/Key.h>
+#include <gtsam/linear/iterative-inl.h>
+#include <gtsam/inference/EliminationTree-inl.h>
+
 
 using namespace std;
 
-namespace gtsam {}
+namespace gtsam {
+
+template<class GRAPH, class LINEAR, class VALUES>
+typename SubgraphSolver<GRAPH,LINEAR,VALUES>::shared_ptr
+SubgraphSolver<GRAPH,LINEAR,VALUES>::update(const LINEAR &graph) const {
+
+	shared_linear Ab1 = boost::make_shared<LINEAR>(),
+				  Ab2 = boost::make_shared<LINEAR>();
+
+	if (parameters_->verbosity()) cout << "split the graph ...";
+	graph.split(pairs_, *Ab1, *Ab2) ;
+	if (parameters_->verbosity()) 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, parameters_) ;
+}
+
+template<class GRAPH, class LINEAR, class VALUES>
+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_);
+
+	boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
+	*xbar = pc_->x(ybar);
+	return xbar;
+}
+
+template<class GRAPH, class LINEAR, class VALUES>
+void SubgraphSolver<GRAPH,LINEAR,VALUES>::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])) ;
+		}
+	}
+
+
+
+
+
+
+
+
+
+}

linear/SubgraphSolver.h

@@ -11,17 +11,12 @@
 
 #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/IterativeSolver.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 {
@@ -33,7 +28,7 @@ namespace gtsam {
    *   solve : L -> VectorValues
    */
 	template<class GRAPH, class LINEAR, class VALUES>
-	class SubgraphSolver {
+	class SubgraphSolver : public IterativeSolver {
 
 	private:
 		typedef typename VALUES::Key Key;
@@ -48,11 +43,6 @@ namespace gtsam {
 		typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
 		typedef std::map<Index,Index> mapPairIndex ;
 
-		// TODO not hardcode
-		static const size_t maxIterations_=100;
-		static const double epsilon_=1e-4, epsilon_abs_=1e-5;
-		static const bool verbose_=true;
-
 		/* the ordering derived from the spanning tree */
 		shared_ordering ordering_;
 
@@ -63,86 +53,36 @@ namespace gtsam {
 		shared_preconditioner pc_;
 
 	public:
-	  	SubgraphSolver(){}
 
 	  	SubgraphSolver(const LINEAR &GFG) {
 			throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
 	  	}
 
 	  	SubgraphSolver(const SubgraphSolver& solver) :
-	  		ordering_(solver.ordering_), pairs_(solver.pairs_),	pc_(solver.pc_){}
+	  		IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_){}
 
 	  	SubgraphSolver(shared_ordering ordering,
 	  				   mapPairIndex pairs,
-	  				   shared_preconditioner pc) :
+	  				   shared_preconditioner pc,
-	  		ordering_(ordering), pairs_(pairs), pc_(pc) {}
+	  				   sharedParameters parameters = boost::make_shared<Parameters>()) :
+	  		IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc) {}
 
-
+		SubgraphSolver(const GRAPH& G, const VALUES& theta0):IterativeSolver(){
-		SubgraphSolver(const GRAPH& G, const VALUES& theta0) { initialize(G,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_ptr update(const LINEAR &graph) const ;
+		VectorValues::shared_ptr optimize() const ;
 		shared_ordering ordering() const { return ordering_; }
 
+
 	protected:
 
-		void initialize(const GRAPH& G, const VALUES& theta0) {
+		void initialize(const GRAPH& G, const VALUES& theta0);
-			// generate spanning tree
-			PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
 
-			// make the ordering
+	private:
-			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])) ;
-			}
-		}
 
+	  	SubgraphSolver():IterativeSolver(){}
 	};
 
-	template<class GRAPH, class LINEAR, class VALUES> const size_t SubgraphSolver<GRAPH, LINEAR, VALUES>::maxIterations_;
-	template<class GRAPH, class LINEAR, class VALUES> const bool SubgraphSolver<GRAPH, LINEAR, VALUES>::verbose_;
-	template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_;
-	template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_abs_;
-
 } // nsamespace gtsam

linear/iterative-inl.h

@@ -18,6 +18,8 @@
 
 #pragma once
 
+#include <gtsam/linear/IterativeOptimizationParameters.h>
+#include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/iterative.h>
 
@@ -30,21 +32,21 @@ namespace gtsam {
 	template<class S, class V, class E>
 	struct CGState {
 
-		bool steepest, verbose;
+		typedef IterativeSolver::Parameters Parameters;
-		double gamma, threshold;
+		typedef IterativeSolver::sharedParameters sharedParameters;
-		size_t k, maxIterations, reset;
+
+		const sharedParameters parameters_;
+
+		int k;
+		bool steepest;
 		V g, d;
+		double gamma, threshold;
 		E Ad;
 
 		/* ************************************************************************* */
 		// Constructor
-		CGState(const S& Ab, const V& x, bool verb, double epsilon,
+		CGState(const S& Ab, const V& x, const sharedParameters parameters, bool steep):
-				double epsilon_abs, size_t maxIt, bool steep) {
+		parameters_(parameters),k(0),steepest(steep) {
-			k = 0;
-			verbose = verb;
-			steepest = steep;
-			maxIterations = (maxIt > 0) ? maxIt : dim(x) * (steepest ? 10 : 1);
-			reset = (size_t) (sqrt(dim(x)) + 0.5); // when to reset
 
 			// Start with g0 = A'*(A*x0-b), d0 = - g0
 			// i.e., first step is in direction of negative gradient
@@ -53,10 +55,10 @@ namespace gtsam {
 
 			// init gamma and calculate threshold
 			gamma = dot(g,g) ;
-			threshold = ::max(epsilon_abs, epsilon * epsilon * gamma);
+			threshold = ::max(parameters->epsilon_abs(), parameters->epsilon() * parameters->epsilon() * gamma);
 
 			// Allocate and calculate A*d for first iteration
-			if (gamma > epsilon) Ad = Ab * d;
+			if (gamma > parameters->epsilon_abs()) Ad = Ab * d;
 		}
 
 		/* ************************************************************************* */
@@ -82,34 +84,32 @@ namespace gtsam {
 		/* ************************************************************************* */
 		// take a step, return true if converged
 		bool step(const S& Ab, V& x) {
-			k += 1; // increase iteration number
+			if ((++k) >= parameters_->maxIterations()) return true;
 
+			//---------------------------------->
 			double alpha = takeOptimalStep(x);
 
-			if (k >= maxIterations) return true; //---------------------------------->
-
 			// update gradient (or re-calculate at reset time)
-			if (k % reset == 0)
+			if (k % parameters_->reset() == 0) g = Ab.gradient(x);
-				g = Ab.gradient(x);
+			// axpy(alpha, Ab ^ Ad, g);  // g += alpha*(Ab^Ad)
-			else
+			else Ab.transposeMultiplyAdd(alpha, Ad, g);
-				// axpy(alpha, Ab ^ Ad, g);  // g += alpha*(Ab^Ad)
-				Ab.transposeMultiplyAdd(alpha, Ad, g);
 
 			// check for convergence
 			double new_gamma = dot(g, g);
-			if (verbose) cout << "iteration " << k << ": alpha = " << alpha
+			if (parameters_->verbosity())
-					<< ", dotg = " << new_gamma << endl;
+				cout << "iteration " << k << ": alpha = " << alpha
-			if (new_gamma < threshold) return true; //---------------------------------->
+				     << ", dotg = " << new_gamma << endl;
+			if (new_gamma < threshold) return true;
 
 			// calculate new search direction
-			if (steepest)
+			if (steepest) d = g;
-				d = g;
 			else {
 				double beta = new_gamma / gamma;
 				// d = g + d*beta;
 				scal(beta, d);
 				axpy(1.0, g, d);
 			}
+
 			gamma = new_gamma;
 
 			// In-place recalculation Ad <- A*d to avoid re-allocating Ad
@@ -123,23 +123,25 @@ namespace gtsam {
 	// conjugate gradient method.
 	// S: linear system, V: step vector, E: errors
 	template<class S, class V, class E>
-	V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon,
+	V conjugateGradients(
-			double epsilon_abs, size_t maxIterations, bool steepest = false) {
+			const S& Ab, V x,
+			const IterativeSolver::sharedParameters parameters,
+			bool steepest = false) {
 
-		CGState<S, V, E> state(Ab, x, verbose, epsilon, epsilon_abs, maxIterations,steepest);
+		CGState<S, V, E> state(Ab, x, parameters, steepest);
-		if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
+		if (parameters->verbosity())
-				<< state.maxIterations << ", ||g0||^2 = " << state.gamma
+			cout << "CG: epsilon = " << parameters->epsilon()
-				<< ", threshold = " << state.threshold << endl;
+				 << ", maxIterations = " << parameters->maxIterations()
+				 << ", ||g0||^2 = " << state.gamma
+				 << ", threshold = " << state.threshold << endl;
 
-		if (state.gamma < state.threshold) {
+		if ( state.gamma < state.threshold ) {
-			if (verbose) cout << "||g0||^2 < threshold, exiting immediately !" << endl;
+			if (parameters->verbosity()) cout << "||g0||^2 < threshold, exiting immediately !" << endl;
 			return x;
 		}
 
 		// loop maxIterations times
-		while (!state.step(Ab, x))
+		while (!state.step(Ab, x)) {}
-			;
-
 		return x;
 	}
 

linear/iterative.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/IterativeOptimizationParameters.h>
 #include <gtsam/linear/iterative-inl.h>
 
 using namespace std;
@@ -34,46 +35,75 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	Vector steepestDescent(const System& Ab, const Vector& x, bool verbose,
+
-			double epsilon, double epsilon_abs, size_t maxIterations) {
+	Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) {
-		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
+		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
-				epsilon_abs, maxIterations, true);
 	}
 
-	Vector conjugateGradientDescent(const System& Ab, const Vector& x,
+	Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) {
-			bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
-		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
-				epsilon_abs, maxIterations);
 	}
 
 	/* ************************************************************************* */
-	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
+	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) {
-			bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
 		System Ab(A, b);
-		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
+		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
-				epsilon_abs, maxIterations, true);
 	}
 
-	Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
+	Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) {
-			const Vector& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
 		System Ab(A, b);
-		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
+		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
-				epsilon_abs, maxIterations);
 	}
 
 	/* ************************************************************************* */
-	VectorValues steepestDescent(const GaussianFactorGraph& fg,
+	VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) {
-			const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters, true);
-		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
-				x, verbose, epsilon, epsilon_abs, maxIterations, true);
 	}
 
-	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
+	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) {
-			const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters);
-		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
-				x, verbose, epsilon, epsilon_abs, maxIterations);
 	}
 
+//	Vector steepestDescent(const System& Ab, const Vector& x, bool verbose,
+//			double epsilon, double epsilon_abs, size_t maxIterations) {
+//		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
+//				epsilon_abs, maxIterations, true);
+//	}
+//
+//	Vector conjugateGradientDescent(const System& Ab, const Vector& x,
+//			bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+//		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
+//				epsilon_abs, maxIterations);
+//	}
+//
+//	/* ************************************************************************* */
+//	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
+//			bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+//		System Ab(A, b);
+//		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
+//				epsilon_abs, maxIterations, true);
+//	}
+//
+//	Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
+//			const Vector& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+//		System Ab(A, b);
+//		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
+//				epsilon_abs, maxIterations);
+//	}
+//
+//	/* ************************************************************************* */
+//	VectorValues steepestDescent(const GaussianFactorGraph& fg,
+//			const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+//		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
+//				x, verbose, epsilon, epsilon_abs, maxIterations, true);
+//	}
+//
+//	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
+//			const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
+//		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
+//				x, verbose, epsilon, epsilon_abs, maxIterations);
+//	}
+
 	/* ************************************************************************* */
 
 } // namespace gtsam

linear/iterative.h

@@ -20,6 +20,7 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/IterativeSolver.h>
 
 namespace gtsam {
 
@@ -87,46 +88,35 @@ namespace gtsam {
 	/**
 	 * Method of steepest gradients, System version
 	 */
-	Vector steepestDescent(const System& Ab, const Vector& x, bool verbose =
+	Vector steepestDescent(const System& Ab, const Vector& x,const IterativeSolver::sharedParameters parameters);
-			false, double epsilon = 1e-3, double epsilon_abs = 1e-5, size_t maxIterations = 0);
 
 	/**
 	 * Method of conjugate gradients (CG), System version
 	 */
-	Vector conjugateGradientDescent(const System& Ab, const Vector& x,
+	Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters);
-			bool verbose = false, double epsilon = 1e-3, double epsilon_abs = 1e-5,
-			size_t maxIterations = 0);
 
 	/** convenience calls using matrices, will create System class internally: */
 
 	/**
 	 * Method of steepest gradients, Matrix version
 	 */
-	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
+	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,const IterativeSolver::sharedParameters parameters);
-			bool verbose = false, double epsilon = 1e-3, double epsilon_abs = 1e-5,
-			size_t maxIterations = 0);
 
 	/**
 	 * Method of conjugate gradients (CG), Matrix version
 	 */
-	Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
+	Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x,const IterativeSolver::sharedParameters parameters);
-			const Vector& x, bool verbose = false, double epsilon = 1e-3,
-			double epsilon_abs = 1e-5, size_t maxIterations = 0);
 
 	class GaussianFactorGraph;
 
 	/**
 	 * Method of steepest gradients, Gaussian Factor Graph version
 	 * */
-	VectorValues steepestDescent(const GaussianFactorGraph& fg,
+	VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters);
-			const VectorValues& x, bool verbose = false, double epsilon = 1e-3,
-			double epsilon_abs = 1e-5, size_t maxIterations = 0);
 
 	/**
 	 * Method of conjugate gradients (CG), Gaussian Factor Graph version
 	 * */
-	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
+	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters);
-			const VectorValues& x, bool verbose = false, double epsilon = 1e-3,
-			double epsilon_abs = 1e-5, size_t maxIterations = 0);
 
 } // namespace gtsam

nonlinear/NonlinearOptimization-inl.h

@@ -19,8 +19,11 @@
 
 #pragma once
 
+#include <boost/make_shared.hpp>
+
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/linear/ConjugateGradientSolver.h>
 #include <gtsam/linear/SubgraphSolver-inl.h>
 #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
@@ -67,6 +70,31 @@ namespace gtsam {
 		return *result.values();
 	}
 
+	/**
+	 * The cg solver
+	 */
+	template<class G, class T>
+	T optimizeCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
+
+		typedef ConjugateGradientSolver<G,GaussianFactorGraph,T> Solver;
+		typedef boost::shared_ptr<Solver> sharedSolver;
+		typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> Optimizer;
+
+		Ordering::shared_ptr ordering = initialEstimate.orderingArbitrary() ;
+		sharedSolver solver = boost::make_shared<Solver>(graph, initialEstimate, *ordering, boost::make_shared<IterativeOptimizationParameters>());
+		Optimizer optimizer(
+				boost::make_shared<const G>(graph),
+				boost::make_shared<const T>(initialEstimate),
+				ordering,
+				solver);
+
+		// Levenberg-Marquardt
+		Optimizer result = optimizer.levenbergMarquardt(parameters);
+		return *result.values();
+	}
+
+
+
 	/**
 	 * The sparse preconditioned conjucate gradient solver
 	 */
@@ -100,10 +128,10 @@ namespace gtsam {
 			return optimizeSequential<G,T>(graph, initialEstimate, parameters);
 		case MULTIFRONTAL:
 			return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters);
+		case CG:
+			return optimizeCG<G,T>(graph, initialEstimate, parameters);
 		case SPCG:
 			throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
-
-			break;
 		}
 		throw runtime_error("optimize: undefined solver");
 	}

nonlinear/NonlinearOptimization.h

@@ -37,6 +37,7 @@ namespace gtsam {
 	enum LinearSolver{
 		SEQUENTIAL,      // Sequential elimination
 		MULTIFRONTAL,      // Multi-frontal elimination
+		CG,				  // vanilla conjugate gradient
 		SPCG,			  // Subgraph Preconditioned Conjugate Gradient
 	};
 
@@ -45,7 +46,8 @@ namespace gtsam {
 	 * optimization that returns the values
 	 */
 	template<class G, class T>
-	T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
+	T optimize(const G& graph, const T& initialEstimate,
+			const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
 			const enum LinearSolver& solver = MULTIFRONTAL);
 
 }