added an absolution threshold $epsilon_abs$ to conjugateGradients. added utility functions to several class to have the same interface which can be used by template functions

cpp/Errors.cpp

@@ -60,6 +60,11 @@ double dot(const Errors& a, const Errors& b) {
 	return result;
 }
 
+/* ************************************************************************* */
+void print(const Errors& a, const string& s) {
+	a.print(s);
+}
+
 /* ************************************************************************* */
 
 } // gtsam

cpp/Errors.h

@@ -36,4 +36,7 @@ namespace gtsam {
    */
   double dot(const Errors& a, const Errors& b);
 
+  /** print with optional string */
+  void print(const Errors& a, const std::string& s = "Error");
+
 } // gtsam

cpp/SubgraphPreconditioner.cpp

@@ -98,4 +98,8 @@ namespace gtsam {
 	}
 	/* ************************************************************************* */
 
+	void SubgraphPreconditioner::print(const std::string& s) const {
+		cout << s << endl;
+		Ab2_.print();
+	}
 } // nsamespace gtsam

cpp/SubgraphPreconditioner.h

@@ -54,6 +54,9 @@ namespace gtsam {
 
 		/** Apply operator A' */
 		VectorConfig operator^(const Errors& e) const;
+
+		/** print the object */
+		void print(const std::string& s = "SubgraphPreconditioner") const;
 	};
 
 } // nsamespace gtsam

cpp/VectorConfig.cpp

@@ -198,4 +198,9 @@ double dot(const VectorConfig& a, const VectorConfig& b) {
 }
 /* ************************************************************************* */
 
+void print(const VectorConfig& v, const std::string& s){
+	v.print(s);
+}
+/* ************************************************************************* */
+
 } // gtsam

cpp/VectorConfig.h

@@ -125,4 +125,7 @@ namespace gtsam {
   /** dim function (for iterative::CGD) */
   inline double dim(const VectorConfig& x) { return x.dim();}
 
+  /** print with optional string */
+  void print(const VectorConfig& v, const std::string& s = "");
+
 } // gtsam

cpp/iterative-inl.h

@@ -15,8 +15,12 @@ using namespace std;
 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(const S& Ab, V x, bool verbose, double epsilon, double epsilon_abs,
 			size_t maxIterations, bool steepest = false) {
 
 		if (maxIterations == 0) maxIterations = dim(x) * (steepest ? 10 : 1);
@@ -27,6 +31,7 @@ namespace gtsam {
 		V g = Ab.gradient(x);
 		V d = -g;
 		double dotg0 = dot(g, g), prev_dotg = dotg0;
+		if (dotg0 < epsilon_abs) return x;
 		double threshold = epsilon * epsilon * dotg0;
 
 		if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "

cpp/iterative.cpp

@@ -4,7 +4,10 @@
  * @author Frank Dellaert
  * Created on: Dec 28, 2009
  */
+#include <iostream>
 
+#include "Vector.h"
+#include "Matrix.h"
 #include "GaussianFactorGraph.h"
 #include "iterative-inl.h"
 
@@ -12,45 +15,52 @@ using namespace std;
 
 namespace gtsam {
 
+	/* ************************************************************************* */
+	void System::print (const string& s) const {
+		cout << s << endl;
+		gtsam::print(A_, "A");
+		gtsam::print(b_, "b");
+	}
+
 	/* ************************************************************************* */
 	Vector steepestDescent(const System& Ab, const Vector& x, bool verbose,
-			double epsilon, size_t maxIterations) {
+			double epsilon, double epsilon_abs, size_t maxIterations) {
 		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
-				maxIterations, true);
+				epsilon_abs, maxIterations, true);
 	}
 
 	Vector conjugateGradientDescent(const System& Ab, const Vector& x,
-			bool verbose, double epsilon, size_t maxIterations) {
+			bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
 		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
-				maxIterations);
+				epsilon_abs, maxIterations);
 	}
 
 	/* ************************************************************************* */
 	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
-			bool verbose, double epsilon, size_t maxIterations) {
+			bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
 		System Ab(A, b);
 		return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
-				maxIterations, true);
+				epsilon_abs, maxIterations, true);
 	}
 
 	Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
-			const Vector& x, bool verbose, double epsilon, size_t maxIterations) {
+			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,
-				maxIterations);
+				epsilon_abs, maxIterations);
 	}
 
 	/* ************************************************************************* */
 	VectorConfig steepestDescent(const GaussianFactorGraph& fg,
-			const VectorConfig& x, bool verbose, double epsilon, size_t maxIterations) {
+			const VectorConfig& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
 		return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg,
-				x, verbose, epsilon, maxIterations, true);
+				x, verbose, epsilon, epsilon_abs, maxIterations, true);
 	}
 
 	VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg,
-			const VectorConfig& x, bool verbose, double epsilon, size_t maxIterations) {
+			const VectorConfig& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
 		return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg,
-				x, verbose, epsilon, maxIterations);
+				x, verbose, epsilon, epsilon_abs, maxIterations);
 	}
 
 	/* ************************************************************************* */

cpp/iterative.h

@@ -56,19 +56,24 @@ namespace gtsam {
 			return A_ ^ e;
 		}
 
+		/**
+		 * Print with optional string
+		 */
+		void print (const std::string& s = "System") const;
 	};
 
 	/**
 	 * Method of steepest gradients, System version
 	 */
 	Vector steepestDescent(const System& Ab, const Vector& x, bool verbose =
-			false, double epsilon = 1e-3, size_t maxIterations = 0);
+			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,
-			bool verbose = false, double epsilon = 1e-3, size_t maxIterations = 0);
+			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: */
 
@@ -76,14 +81,15 @@ namespace gtsam {
 	 * Method of steepest gradients, Matrix version
 	 */
 	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
-			bool verbose = false, double epsilon = 1e-3, size_t maxIterations = 0);
+			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,
 			const Vector& x, bool verbose = false, double epsilon = 1e-3,
-			size_t maxIterations = 0);
+			double epsilon_abs = 1e-5, size_t maxIterations = 0);
 
 	class GaussianFactorGraph;
 	class VectorConfig;
@@ -93,13 +99,13 @@ namespace gtsam {
 	 * */
 	VectorConfig steepestDescent(const GaussianFactorGraph& fg,
 			const VectorConfig& x, bool verbose = false, double epsilon = 1e-3,
-			size_t maxIterations = 0);
+			double epsilon_abs = 1e-5, size_t maxIterations = 0);
 
 	/**
 	 * Method of conjugate gradients (CG), Gaussian Factor Graph version
 	 * */
 	VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg,
 			const VectorConfig& x, bool verbose = false, double epsilon = 1e-3,
-			size_t maxIterations = 0);
+			double epsilon_abs = 1e-5, size_t maxIterations = 0);
 
 } // namespace gtsam

cpp/testBayesNetPreconditioner.cpp

@@ -94,7 +94,7 @@ TEST( BayesNetPreconditioner, conjugateGradients )
 	double epsilon = 1e-6; // had to crank this down !!!
 	size_t maxIterations = 100;
 	VectorConfig actual_y = gtsam::conjugateGradients<BayesNetPreconditioner,
-			VectorConfig, Errors>(system, y1, verbose, epsilon, maxIterations);
+			VectorConfig, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations);
 	VectorConfig actual_x = system.x(actual_y);
 	CHECK(assert_equal(xtrue,actual_x));
 

cpp/testSubgraphPreconditioner.cpp

@@ -195,7 +195,7 @@ TEST( SubgraphPreconditioner, conjugateGradients )
 	double epsilon = 1e-3;
 	size_t maxIterations = 100;
 	VectorConfig actual = gtsam::conjugateGradients<SubgraphPreconditioner,
-			VectorConfig, Errors>(system, y1, verbose, epsilon, maxIterations);
+			VectorConfig, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations);
 	CHECK(assert_equal(y0,actual));
 
 	// Compare with non preconditioned version: