diff --git a/cpp/Errors.cpp b/cpp/Errors.cpp index af76b5d49..12749a869 100644 --- a/cpp/Errors.cpp +++ b/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 diff --git a/cpp/Errors.h b/cpp/Errors.h index 37fe49dcf..ca0bc1c60 100644 --- a/cpp/Errors.h +++ b/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 diff --git a/cpp/SubgraphPreconditioner.cpp b/cpp/SubgraphPreconditioner.cpp index 29f35f063..c42d80a61 100644 --- a/cpp/SubgraphPreconditioner.cpp +++ b/cpp/SubgraphPreconditioner.cpp @@ -98,4 +98,8 @@ namespace gtsam { } /* ************************************************************************* */ + void SubgraphPreconditioner::print(const std::string& s) const { + cout << s << endl; + Ab2_.print(); + } } // nsamespace gtsam diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index 3e92b3d67..0a219575a 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/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 diff --git a/cpp/VectorConfig.cpp b/cpp/VectorConfig.cpp index 02aff85b0..04e5807bd 100644 --- a/cpp/VectorConfig.cpp +++ b/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 diff --git a/cpp/VectorConfig.h b/cpp/VectorConfig.h index 3084f9eb3..16c98cd11 100644 --- a/cpp/VectorConfig.h +++ b/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 diff --git a/cpp/iterative-inl.h b/cpp/iterative-inl.h index 6e73d8031..be7a48aa5 100644 --- a/cpp/iterative-inl.h +++ b/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 - 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 = " diff --git a/cpp/iterative.cpp b/cpp/iterative.cpp index 712aad93f..375642f0d 100644 --- a/cpp/iterative.cpp +++ b/cpp/iterative.cpp @@ -4,7 +4,10 @@ * @author Frank Dellaert * Created on: Dec 28, 2009 */ +#include +#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 (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 (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 (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 (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 (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 (fg, - x, verbose, epsilon, maxIterations); + x, verbose, epsilon, epsilon_abs, maxIterations); } /* ************************************************************************* */ diff --git a/cpp/iterative.h b/cpp/iterative.h index dbfbeb6b9..6f7cb0719 100644 --- a/cpp/iterative.h +++ b/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 diff --git a/cpp/testBayesNetPreconditioner.cpp b/cpp/testBayesNetPreconditioner.cpp index a5a4e9974..7d378abe6 100644 --- a/cpp/testBayesNetPreconditioner.cpp +++ b/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(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)); diff --git a/cpp/testSubgraphPreconditioner.cpp b/cpp/testSubgraphPreconditioner.cpp index a67b24fd1..9236f6e13 100644 --- a/cpp/testSubgraphPreconditioner.cpp +++ b/cpp/testSubgraphPreconditioner.cpp @@ -195,7 +195,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) double epsilon = 1e-3; size_t maxIterations = 100; VectorConfig actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, maxIterations); + VectorConfig, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations); CHECK(assert_equal(y0,actual)); // Compare with non preconditioned version: