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
							parent
							
								
									1b53a240b9
								
							
						
					
					
						commit
						40889e8f50
					
				|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -98,4 +98,8 @@ namespace gtsam { | |||
| 	} | ||||
| 	/* ************************************************************************* */ | ||||
| 
 | ||||
| 	void SubgraphPreconditioner::print(const std::string& s) const { | ||||
| 		cout << s << endl; | ||||
| 		Ab2_.print(); | ||||
| 	} | ||||
| } // nsamespace gtsam
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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 = " | ||||
|  |  | |||
|  | @ -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); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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)); | ||||
| 
 | ||||
|  |  | |||
|  | @ -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:
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue