add preconditioned conjugate gradient

examples/Makefile.am

@@ -16,6 +16,7 @@ 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 += Pose2SLAMwCG				  		# Solves a simple Pose2 SLAM example with easy CG 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
 SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses

examples/Pose2SLAMwCG.cpp

@@ -0,0 +1,84 @@
+/*
+ * Pose2SLAMwCG.cpp
+ *
+ *  Created on: Oct 27, 2010
+ *      Author: Yong-Dian Jian
+ */
+
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
+ */
+
+
+#include <boost/shared_ptr.hpp>
+
+#include <gtsam/slam/pose2SLAM.h>
+
+#include <gtsam/linear/Preconditioner.h>
+#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace pose2SLAM;
+
+Graph graph;
+Values initial;
+Values result;
+
+/* ************************************************************************* */
+int main(void) {
+
+	/* generate synthetic data */
+	const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
+	Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
+
+	// create a 3 by 3 grid
+	// x3 x6 x9
+	// x2 x5 x8
+	// x1 x4 x7
+	graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
+	graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
+	graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
+	graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
+	graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
+	graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
+	graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
+	graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
+	graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
+	graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
+	graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
+	graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
+	graph.addPrior(x1, Pose2(0,0,0), sigma) ;
+
+	initial.insert(x1, Pose2(  0,  0,   0));
+	initial.insert(x2, Pose2(  0, 2.1, 0.01));
+	initial.insert(x3, Pose2(  0, 3.9,-0.01));
+	initial.insert(x4, Pose2(2.1,-0.1,    0));
+	initial.insert(x5, Pose2(1.9, 2.1, 0.02));
+	initial.insert(x6, Pose2(2.0, 3.9,-0.02));
+	initial.insert(x7, Pose2(4.0, 0.1, 0.03 ));
+	initial.insert(x8, Pose2(3.9, 2.1, 0.01));
+	initial.insert(x9, Pose2(4.1, 3.9,-0.01));
+	/* done */
+
+
+	graph.print("full graph") ;
+	initial.print("initial estimate");
+
+	result = optimizePCG<Graph,Values, JacobiPreconditioner<GaussianFactorGraph,VectorValues> >(graph, initial);
+	//result = optimizeCG(graph, initial);
+	result.print("final result") ;
+	return 0 ;
+}
+

gtsam/base/Matrix.cpp

@@ -399,6 +399,17 @@ void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) {
 	colsubproxy = col;
 }
 
+/* ************************************************************************* */
+
+Vector columnNormSquare(const MatrixColMajor &A) {
+	Vector v (A.size2()) ;
+	for ( size_t i = 0 ; i < A.size2() ; ++i ) {
+		ublas::matrix_column<const MatrixColMajor> mc (A, i);
+		v[i] = dot(mc, mc) ;
+	}
+	return v ;
+}
+
 /* ************************************************************************* */
 void solve(Matrix& A, Matrix& B)
 {

gtsam/base/Matrix.h

@@ -205,6 +205,8 @@ Vector column_(const Matrix& A, size_t j);
 void insertColumn(Matrix& A, const Vector& col, size_t j);
 void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j);
 
+Vector columnNormSquare(const MatrixColMajor &A) ;
+
 /**
  * extracts a row from a matrix
  * @param matrix to extract row from

gtsam/linear/ConjugateGradientSolver.h

@@ -15,7 +15,6 @@
 
 namespace gtsam {
 
-	// GaussianFactorGraph, Values
  	template<class GRAPH, class LINEAR, class VALUES>
 	class ConjugateGradientSolver : public IterativeSolver {
 
@@ -33,30 +32,104 @@ namespace gtsam {
 
 		typedef boost::shared_ptr<const ConjugateGradientSolver> shared_ptr ;
 
+		// suggested constructor
 		ConjugateGradientSolver(const GRAPH &graph, const VALUES &initial, const Ordering &ordering, const Parameters &parameters = Parameters()):
 			IterativeSolver(parameters), ptr_(0), zeros_(boost::make_shared<VectorValues>(initial.zero(ordering))) {}
 
+		// to be compatible to NonlinearOptimizer, but shouldn't be called...
 		ConjugateGradientSolver(const LINEAR &GFG) {
 			std::cout << "[ConjugateGradientSolver] Unexpected usage.." << std::endl;
 			throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
 	  	}
 
+		// copy constructor
 		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 {
-			VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, parameters_);
+			VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, *parameters_);
 			return boost::make_shared<VectorValues>(x) ;
 		}
 
-	private:
-		ConjugateGradientSolver():IterativeSolver(),ptr_(0){}
 
+		// for NonlinearOptimizer update only
+		ConjugateGradientSolver(const sharedParameters parameters,
+				                const LINEAR *ptr,
+				                const sharedVectorValues zeros):
+			IterativeSolver(parameters), ptr_(ptr), zeros_(zeros) {}
+
+	private:
+		// shouldn't be used
+		ConjugateGradientSolver(){}
 	};
 
+ 	template<class GRAPH, class LINEAR, class VALUES, class PC>
+	class PreconditionedConjugateGradientSolver : public IterativeSolver {
+
+	protected:
+
+		typedef boost::shared_ptr<GRAPH> sharedGRAPH ;
+		typedef boost::shared_ptr<LINEAR> sharedLINEAR ;
+		typedef boost::shared_ptr<PC> sharedPreconditioner ;
+		typedef boost::shared_ptr<VALUES> sharedVALUES ;
+		typedef boost::shared_ptr<VectorValues> sharedVectorValues ;
+
+		const LINEAR *ptr_;
+		sharedPreconditioner pc_ ;
+		sharedVectorValues zeros_;
+	public:
+
+		typedef boost::shared_ptr<const PreconditionedConjugateGradientSolver> shared_ptr ;
+
+		// suggested constructor
+		PreconditionedConjugateGradientSolver(
+				const GRAPH &graph,
+				const VALUES &initial,
+				const Ordering &ordering,
+				const Parameters &parameters = Parameters()):
+			IterativeSolver(parameters), ptr_(0), pc_(), zeros_(boost::make_shared<VectorValues>(initial.zero(ordering))) {}
+
+		// to be compatible to NonlinearOptimizer, but shouldn't be called...
+		PreconditionedConjugateGradientSolver(const LINEAR &GFG) {
+			std::cout << "[PreconditionedConjugateGradientSolver] Unexpected usage.." << std::endl;
+			throw std::runtime_error("PreconditionedConjugateGradientSolver: gaussian factor graph initialization not supported");
+	  	}
+
+		// copy
+		PreconditionedConjugateGradientSolver(const PreconditionedConjugateGradientSolver& solver) :
+			IterativeSolver(solver), ptr_(solver.ptr_), pc_(solver.pc_), zeros_(solver.zeros_){}
+
+		// called by NonlinearOptimizer
+		shared_ptr update(const LINEAR &graph) const {
+
+			return boost::make_shared<PreconditionedConjugateGradientSolver>(
+					parameters_,
+					&graph,
+					boost::make_shared<PC>(&graph, parameters_, zeros_),
+					zeros_) ;
+		}
+
+		// optimize with preconditioned conjugate gradient
+		VectorValues::shared_ptr optimize() const {
+			VectorValues x = preconditionedConjugateGradientDescent<LINEAR, PC, VectorValues>
+					(*ptr_, *pc_, *zeros_, *parameters_);
+			return boost::make_shared<VectorValues>(x) ;
+		}
+
+		PreconditionedConjugateGradientSolver(
+				const sharedParameters parameters,
+				const LINEAR *ptr,
+				const sharedPreconditioner pc,
+				const sharedVectorValues zeros):
+			IterativeSolver(parameters), ptr_(ptr), pc_(pc), zeros_(zeros) {}
+	private:
+
+		// shouldn't be used
+		PreconditionedConjugateGradientSolver(){}
+	};
+
+
 } // nsamespace gtsam

gtsam/linear/GaussianFactorGraph.cpp

@@ -195,18 +195,73 @@ bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorG
 	return true ;
 }
 
-boost::shared_ptr<VectorValues> GaussianFactorGraph::allocateVectorVavlues() const {
+VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
 	std::vector<size_t> dimensions(size()) ;
-	BOOST_FOREACH( const sharedFactor& factor, factors_) {
 	Index i = 0 ;
-		BOOST_FOREACH( const Index& idx, factor->keys_) {
+	BOOST_FOREACH( const sharedFactor& factor, factors_) {
-			dimensions[idx] = factor->Ab_(i).size2() ;
+		dimensions[i] = factor->numberOfRows() ;
 		i++;
 	}
-	}
 
-	return boost::make_shared<VectorValues>(dimensions) ;
+	return VectorValues(dimensions) ;
 }
 
 
+bool GaussianFactorGraph::getDiagonalOfHessian(VectorValues &values) const {
+
+	values.makeZero() ;
+
+	BOOST_FOREACH( const sharedFactor& factor, factors_) {
+		Index i = 0 ;
+		BOOST_FOREACH( const Index& idx, factor->keys_) {
+			Vector v = columnNormSquare(factor->Ab_(i)) ;
+			values[idx] += v;
+		}
+	}
+	return true ;
+}
+
+void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const {
+
+	getb(r) ;
+	VectorValues Ax = VectorValues::SameStructure(r) ;
+	multiply(x,Ax) ;
+	axpy(-1.0,Ax,r) ;
+}
+
+void GaussianFactorGraph::multiply(const VectorValues &x, VectorValues &r) const {
+
+	r.makeZero() ;
+	Index i = 0 ;
+	BOOST_FOREACH(const sharedFactor& factor, factors_) {
+		Index j = 0 ;
+		BOOST_FOREACH( const Index& idx, factor->keys_ ) {
+			r[i] += prod(factor->Ab_(j), x[idx]) ;
+			++j ;
+		}
+		++i ;
+	}
+}
+
+void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues &x) const {
+	x.makeZero() ;
+	Index i = 0 ;
+	BOOST_FOREACH(const sharedFactor& factor, factors_) {
+		Index j = 0 ;
+		BOOST_FOREACH( const Index& idx, factor->keys_ ) {
+			x[idx] += prod(trans(factor->Ab_(j)), r[i]) ;
+			++j ;
+		}
+		++i ;
+	}
+}
+
+void GaussianFactorGraph::getb(VectorValues &b) const {
+	Index i = 0 ;
+	BOOST_FOREACH( const sharedFactor& factor, factors_) {
+		b[i] = factor->getb() ;
+		i++;
+	}
+}
+
 } // namespace gtsam

gtsam/linear/GaussianFactorGraph.h

@@ -152,14 +152,22 @@ 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 ;
-	boost::shared_ptr<VectorValues> allocateVectorVavlues() 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 ;
+
+	void residual(const VectorValues &x, VectorValues &r) const ;
+	void multiply(const VectorValues &x, VectorValues &r) const ;
+	void transposeMultiply(const VectorValues &r, VectorValues &x) const ;
+	void getb(VectorValues &b) const ;
   };
 
 } // namespace gtsam

gtsam/linear/IterativeOptimizationParameters.h

@@ -32,7 +32,7 @@ namespace gtsam {
 			reset_(101),
 			epsilon_(1e-5),
 			epsilon_abs_(1e-5),
-			verbosity_(SILENT) {}
+			verbosity_(ERROR) {}
 
 		IterativeOptimizationParameters(const IterativeOptimizationParameters &parameters):
 			maxIterations_(parameters.maxIterations_),
@@ -43,7 +43,7 @@ namespace gtsam {
 
 
 		IterativeOptimizationParameters
-		(int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=SILENT, int reset=-1):
+		(int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=ERROR, int reset=-1):
 		maxIterations_(maxIterations), reset_(reset),
 		epsilon_(epsilon), epsilon_abs_(epsilon_abs), verbosity_(verbosity) {
 			if (reset_==-1) reset_ = maxIterations_ + 1 ;

gtsam/linear/Preconditioner.h

@@ -0,0 +1,88 @@
+/*
+ * Preconditioner.h
+ *
+ *  Created on: Oct 27, 2010
+ *      Author: Yong-Dian Jian
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/IterativeOptimizationParameters.h>
+
+namespace gtsam {
+
+template <class LINEAR, class VALUES>
+class Preconditioner {
+
+public:
+	typedef IterativeOptimizationParameters Parameters;
+	typedef boost::shared_ptr<Parameters> sharedParameters;
+	typedef boost::shared_ptr<LINEAR> sharedLinear;
+	typedef boost::shared_ptr<VALUES> sharedValues ;
+
+public:
+	Preconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros):
+		pLinear_(ptr), parameters_(parameters), zeros_(zeros){}
+
+	void solve(const VALUES &values, VALUES &result) const { result = values ; }
+	void solveTranspose(const VALUES &values, VALUES &result) const { result = values ; }
+
+protected:
+	const LINEAR* pLinear_ ;
+	const sharedParameters parameters_ ;
+	const sharedValues zeros_;
+
+private:
+	Preconditioner(){}
+};
+
+template <class LINEAR, class VALUES>
+class JacobiPreconditioner : public Preconditioner<LINEAR,VALUES> {
+
+public:
+
+	typedef Preconditioner<LINEAR,VALUES> Base ;
+	typedef IterativeOptimizationParameters Parameters;
+	typedef boost::shared_ptr<Parameters> sharedParameters;
+	typedef boost::shared_ptr<LINEAR> sharedLinear;
+	typedef boost::shared_ptr<VALUES> sharedValues ;
+
+
+	JacobiPreconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros):
+		Base(ptr, parameters, zeros), scale_(new VALUES(*zeros)) {
+		initialize() ;
+	}
+
+	void initialize() {
+		Base::pLinear_->getDiagonalOfHessian(*scale_);
+		sqrt(*scale_) ;
+
+		// check if any zero in scale_
+		if ( anyZero(*scale_) ) {
+			std::cout << "[JacobiPreconditioner] some diagonal element of hessian is zero" << std::endl ;
+			throw std::runtime_error("JacobiPreconditioner");
+		}
+	}
+
+	void solve(const VALUES &values, VALUES &result) const {
+		//std::cout << "solve" << std::endl;
+		ediv(values, *scale_, result) ;
+	}
+
+	void solveTranspose(const VALUES &values, VALUES &result) const {
+		//std::cout << "solveTranspose" << std::endl;
+		ediv(values, *scale_, result) ;
+	}
+
+protected:
+	sharedValues scale_ ;
+
+private:
+	JacobiPreconditioner(){}
+};
+
+}

gtsam/linear/SubgraphSolver-inl.h

@@ -58,7 +58,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);

gtsam/linear/VectorValues.h

@@ -194,7 +194,23 @@ public:
   friend double dot(const VectorValues& V1, const VectorValues& V2) { return gtsam::dot(V1.values_, V2.values_) ; }
   friend void scal(double alpha, VectorValues& x) {	gtsam::scal(alpha, x.values_) ; }
   friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { gtsam::axpy(alpha, x.values_, y.values_) ; }
+  friend void sqrt(VectorValues &x) { Vector y = gtsam::esqrt(x.values_); x.values_ = y; }
+  friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) {
+	  Vector v = gtsam::ediv(numerator.values_, denominator.values_);
+	  result.values_ = v ;
+  }
 
+  // check whether there's a zero in the vector
+  friend bool anyZero(const VectorValues& x, double tol=1e-5) {
+	  bool flag = false ;
+	  BOOST_FOREACH(const double &v, x.values_) {
+		  if ( v < tol && v > -tol) {
+			  flag = true ;
+			  break;
+		  }
+	  }
+	  return flag;
+  }
 };
 
 

gtsam/linear/iterative-inl.h

@@ -18,6 +18,9 @@
 
 #pragma once
 
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+
 #include <gtsam/linear/IterativeOptimizationParameters.h>
 #include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
@@ -33,9 +36,7 @@ namespace gtsam {
 	struct CGState {
 
 		typedef IterativeSolver::Parameters Parameters;
-		typedef IterativeSolver::sharedParameters sharedParameters;
+		const Parameters &parameters_;
-
-		const sharedParameters parameters_;
 
 		int k;
 		bool steepest;
@@ -45,7 +46,7 @@ namespace gtsam {
 
 		/* ************************************************************************* */
 		// Constructor
-		CGState(const S& Ab, const V& x, const sharedParameters parameters, bool steep):
+		CGState(const S& Ab, const V& x, const Parameters &parameters, bool steep):
 		parameters_(parameters),k(0),steepest(steep) {
 
 			// Start with g0 = A'*(A*x0-b), d0 = - g0
@@ -55,10 +56,10 @@ namespace gtsam {
 
 			// init gamma and calculate threshold
 			gamma = dot(g,g) ;
-			threshold = ::max(parameters->epsilon_abs(), parameters->epsilon() * parameters->epsilon() * gamma);
+			threshold = ::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
 
 			// Allocate and calculate A*d for first iteration
-			if (gamma > parameters->epsilon_abs()) Ad = Ab * d;
+			if (gamma > parameters_.epsilon_abs()) Ad = Ab * d;
 		}
 
 		/* ************************************************************************* */
@@ -84,19 +85,19 @@ namespace gtsam {
 		/* ************************************************************************* */
 		// take a step, return true if converged
 		bool step(const S& Ab, V& x) {
-			if ((++k) >= parameters_->maxIterations()) return true;
+			if ((++k) >= parameters_.maxIterations()) return true;
 
 			//---------------------------------->
 			double alpha = takeOptimalStep(x);
 
 			// update gradient (or re-calculate at reset time)
-			if (k % parameters_->reset() == 0) g = Ab.gradient(x);
+			if (k % parameters_.reset() == 0) g = Ab.gradient(x);
 			// axpy(alpha, Ab ^ Ad, g);  // g += alpha*(Ab^Ad)
 			else Ab.transposeMultiplyAdd(alpha, Ad, g);
 
 			// check for convergence
 			double new_gamma = dot(g, g);
-			if (parameters_->verbosity())
+			if (parameters_.verbosity())
 				cout << "iteration " << k << ": alpha = " << alpha
 				     << ", dotg = " << new_gamma << endl;
 			if (new_gamma < threshold) return true;
@@ -124,19 +125,22 @@ namespace gtsam {
 	// S: linear system, V: step vector, E: errors
 	template<class S, class V, class E>
 	V conjugateGradients(
-			const S& Ab, V x,
+			const S& Ab,
-			const IterativeSolver::sharedParameters parameters,
+			V x,
+			const IterativeSolver::Parameters &parameters,
 			bool steepest = false) {
 
 		CGState<S, V, E> state(Ab, x, parameters, steepest);
-		if (parameters->verbosity())
+		if (parameters.verbosity())
-			cout << "CG: epsilon = " << parameters->epsilon()
+			cout << "CG: epsilon = " << parameters.epsilon()
-				 << ", maxIterations = " << parameters->maxIterations()
+				 << ", maxIterations = " << parameters.maxIterations()
 				 << ", ||g0||^2 = " << state.gamma
 				 << ", threshold = " << state.threshold << endl;
 
 		if ( state.gamma < state.threshold ) {
-			if (parameters->verbosity()) cout << "||g0||^2 < threshold, exiting immediately !" << endl;
+			if (parameters.verbosity())
+				cout << "||g0||^2 < threshold, exiting immediately !" << endl;
+
 			return x;
 		}
 
@@ -145,6 +149,115 @@ namespace gtsam {
 		return x;
 	}
 
+	/* ************************************************************************* */
+	// state for PCG method
+	template<class LINEAR, class PC, class V>
+	class PCGState {
+
+	public:
+		typedef IterativeSolver::Parameters Parameters;
+		typedef V Values;
+		typedef boost::shared_ptr<Values> sharedValues;
+
+	protected:
+		const LINEAR &Ab_;
+		const PC &pc_ ;
+		V x_ ;
+		const Parameters &parameters_;
+		bool steepest_;
+
+	public:
+		/* ************************************************************************* */
+		// Constructor
+		PCGState(const LINEAR& Ab, const PC &pc, const V &x0, const Parameters &parameters, bool steep):
+			Ab_(Ab), pc_(pc), x_(x0), parameters_(parameters),steepest_(steep) {}
+
+		V run() {
+
+			// refer to Bjorck pp. 294
+			V r = Ab_.allocateVectorValuesb() ;
+			V q = V::SameStructure(r) ;
+
+			V p = V::SameStructure(x_) ;
+			V s = V::SameStructure(x_) ;
+			V t = V::SameStructure(x_) ;
+			V tmp = V::SameStructure(x_) ;
+
+			// initial PCG
+			Ab_.residual(x_, r) ;
+			Ab_.transposeMultiply(r, tmp) ;
+			pc_.solveTranspose(tmp, s) ;
+			p = s ;
+
+			double gamma = dot(s,s), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ;
+
+			const double threshold =
+					::max(parameters_.epsilon_abs(),
+					      parameters_.epsilon() * parameters_.epsilon() * gamma);
+
+			const int iMaxIterations = parameters_.maxIterations();
+			const int iReset = parameters_.reset() ;
+
+			if (parameters_.verbosity())
+				cout << "PCG: epsilon = " << parameters_.epsilon()
+					 << ", maxIterations = " << parameters_.maxIterations()
+					 << ", ||g0||^2 = " << gamma
+					 << ", threshold = " << threshold << endl;
+
+			for ( int k = 0 ; k < iMaxIterations ; ++k ) {
+
+				if ( gamma < threshold ) break ;
+
+				if ( k % iReset == 0) {
+					Ab_.residual(x_, r) ;
+					Ab_.transposeMultiply(r, tmp) ;
+					pc_.solveTranspose(tmp, s) ;
+					p = s ;
+					gamma = dot(s,s) ;
+				}
+
+
+				pc_.solve(p, t) ;
+				Ab_.multiply(t, q) ;
+				alpha = gamma / dot(q,q) ;
+				axpy( alpha, t, x_) ;
+				axpy(-alpha, q, r) ;
+				Ab_.transposeMultiply(r, tmp) ;
+				pc_.solveTranspose(tmp, s) ;
+				new_gamma = dot(s,s) ;
+
+				if (parameters_.verbosity())
+					cout << "iteration " << k
+					     << ": alpha = " << alpha
+					     << ", dotg = " << new_gamma << endl;
+
+				beta = new_gamma / gamma ;
+				scal(beta,p) ;
+				axpy(1.0,s,p) ;
+				gamma = new_gamma ;
+			}
+
+			return x_ ;
+		} // function
+
+	private:
+		PCGState(){}
+
+	}; // PCGState Class
+
+	/* ************************************************************************* */
+	template<class LINEAR, class PC, class V>
+	V preconditionedConjugateGradientDescent(
+			const LINEAR& Ab,
+			const PC& pc,
+			V x,
+			const IterativeSolver::Parameters &parameters,
+			bool steepest = false) {
+
+		PCGState<LINEAR, PC, V> state(Ab, pc, x, parameters, steepest);
+		return state.run() ;
+	}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/linear/iterative.cpp

@@ -20,6 +20,8 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/IterativeSolver.h>
+#include <gtsam/linear/Preconditioner.h>
 #include <gtsam/linear/IterativeOptimizationParameters.h>
 #include <gtsam/linear/iterative-inl.h>
 
@@ -36,74 +38,34 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 
-	Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) {
+	Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) {
 		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
 	}
 
-	Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) {
+	Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) {
 		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
 	}
 
 	/* ************************************************************************* */
-	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) {
+	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) {
 		System Ab(A, b);
 		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
 	}
 
-	Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) {
+	Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) {
 		System Ab(A, b);
 		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
 	}
 
 	/* ************************************************************************* */
-	VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) {
+	VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & parameters) {
 		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters, true);
 	}
 
-	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) {
+	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & parameters) {
 		return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters);
 	}
 
-//	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

gtsam/linear/iterative.h

@@ -21,6 +21,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/IterativeSolver.h>
+#include <gtsam/linear/Preconditioner.h>
 
 namespace gtsam {
 
@@ -88,35 +89,56 @@ namespace gtsam {
 	/**
 	 * Method of steepest gradients, System version
 	 */
-	Vector steepestDescent(const System& Ab, const Vector& x,const IterativeSolver::sharedParameters parameters);
+	Vector steepestDescent(
+			const System& Ab,
+			const Vector& x,
+			const IterativeSolver::Parameters & parameters);
 
 	/**
 	 * Method of conjugate gradients (CG), System version
 	 */
-	Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters);
+	Vector conjugateGradientDescent(
+			const System& Ab,
+			const Vector& x,
+			const IterativeSolver::Parameters & parameters);
 
 	/** 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,const IterativeSolver::sharedParameters parameters);
+	Vector steepestDescent(
+			const Matrix& A,
+			const Vector& b,
+			const Vector& x,
+			const IterativeSolver::Parameters & parameters);
 
 	/**
 	 * Method of conjugate gradients (CG), Matrix version
 	 */
-	Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x,const IterativeSolver::sharedParameters parameters);
+	Vector conjugateGradientDescent(
+			const Matrix& A,
+			const Vector& b,
+			const Vector& x,
+			const IterativeSolver::Parameters & parameters);
 
 	class GaussianFactorGraph;
 
 	/**
 	 * Method of steepest gradients, Gaussian Factor Graph version
 	 * */
-	VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters);
+	VectorValues steepestDescent(
+			const GaussianFactorGraph& fg,
+			const VectorValues& x,
+			const IterativeSolver::Parameters & parameters);
 
 	/**
 	 * Method of conjugate gradients (CG), Gaussian Factor Graph version
 	 * */
-	VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters);
+	VectorValues conjugateGradientDescent(
+			const GaussianFactorGraph& fg,
+			const VectorValues& x,
+			const IterativeSolver::Parameters & parameters);
+
 
 } // namespace gtsam

gtsam/nonlinear/NonlinearOptimization-inl.h

@@ -93,6 +93,27 @@ namespace gtsam {
 		return *result.values();
 	}
 
+	template<class G, class T, class PC>
+	T optimizePCG(const G& graph,
+				  const T& initialEstimate,
+				  const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
+
+		typedef PreconditionedConjugateGradientSolver<G,GaussianFactorGraph,T,PC> 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, 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();
+	}
 
 
 	/**

gtsam/nonlinear/NonlinearOptimizer-inl.h

@@ -357,7 +357,8 @@ namespace gtsam {
 	template<class G, class C, class L, class S, class W>
 	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::
 	levenbergMarquardt(const NonlinearOptimizationParameters &parameters) const {
-		return newParameters_(boost::make_shared<Parameters>(parameters)).levenbergMarquardt() ;
+		boost::shared_ptr<NonlinearOptimizationParameters> ptr (new NonlinearOptimizationParameters(parameters)) ;
+		return newParameters_(ptr).levenbergMarquardt() ;
 	}
 
 	/* ************************************************************************* */