From 1f165a9f85c8482e781ef56fa85286cc2a5651ad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Feb 2010 05:52:20 +0000 Subject: [PATCH] Made CG state a class --- cpp/iterative-inl.h | 115 ++++++++++++++++++++--------- cpp/testIterative.cpp | 15 ++-- cpp/testSubgraphPreconditioner.cpp | 2 +- 3 files changed, 88 insertions(+), 44 deletions(-) diff --git a/cpp/iterative-inl.h b/cpp/iterative-inl.h index 79de39eb1..c327da322 100644 --- a/cpp/iterative-inl.h +++ b/cpp/iterative-inl.h @@ -15,68 +15,111 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - /** - * conjugate gradient method. - * S: linear system, V: step vector, E: errors - */ + // state for CG method template - 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); - size_t reset = (size_t)(sqrt(dim(x))+0.5); // when to reset + struct CGState { - // Start with g0 = A'*(A*x0-b), d0 = - g0 - // i.e., first step is in direction of negative gradient - V g = Ab.gradient(x); - V d = g; // instead of negating gradient, alpha will be negated - double gamma0 = dot(g, g), gamma_old = gamma0; - if (gamma0 < epsilon_abs) return x; - double threshold = epsilon * epsilon * gamma0; + bool steepest, verbose; + double gamma, threshold; + size_t k, maxIterations, reset; + V g, d; + E Ad; - if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = " - << maxIterations << ", ||g0||^2 = " << gamma0 << ", threshold = " - << threshold << endl; + /** constructor */ + CGState(const S& Ab, const V& x, bool verb, double epsilon, + double epsilon_abs, size_t maxIt, bool steep) { + k = 0; + verbose = verb; + steepest = steep; + maxIterations == maxIt ? maxIt : dim(x) * (steepest ? 10 : 1); + reset = (size_t) (sqrt(dim(x)) + 0.5); // when to reset - // Allocate and calculate A*d for first iteration - E Ad = Ab * d; + // Start with g0 = A'*(A*x0-b), d0 = - g0 + // i.e., first step is in direction of negative gradient + g = Ab.gradient(x); + d = g; // instead of negating gradient, alpha will be negated - // loop maxIterations times - for (size_t k = 1;; k++) { + // init gamma and calculate threshold + gamma = dot(g, g); + threshold = ::max(epsilon_abs, epsilon * epsilon * gamma); - // calculate optimal step-size - double alpha = - dot(d, g) / dot(Ad, Ad); + // Allocate and calculate A*d for first iteration + if (gamma > epsilon) Ad = Ab * d; + } - // do step in new search direction - axpy(alpha, d, x); // x += alpha*d - if (k==maxIterations) break; + /** print */ + void print() { + cout << "iteration = " << k << endl; + cout << "dotg = " << gamma << endl; + gtsam::print(g,"g"); + gtsam::print(d,"d"); + gtsam::print(Ad,"Ad"); + } + + /** step the solution */ + double takeOptimalStep(V& x) { + double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size + axpy(alpha, d, x); // // do step in new search direction, x += alpha*d + return alpha; + } + + /** take a step, return true if converged */ + bool step(const S& Ab, V& x) { + k += 1; // increase iteration number + + double alpha = takeOptimalStep(x); + + if (k == maxIterations) return true; //----------------------------------> // update gradient (or re-calculate at reset time) - if (k%reset==0) + if (k % reset == 0) g = Ab.gradient(x); else // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) Ab.transposeMultiplyAdd(alpha, Ad, g); // check for convergence - double gamma = dot(g, g); - if (verbose) cout << "iteration " << k << ": alpha = " << alpha - << ", dotg = " << gamma << endl; - if (gamma < threshold) break; + double new_gamma = dot(g, g); + if (verbose) cout << "iteration " << k << ": alpha = " << alpha << ", dotg = " << new_gamma << endl; + // print(); + if (new_gamma < threshold) return true; //----------------------------------> // calculate new search direction if (steepest) d = g; else { - double beta = gamma / gamma_old; - gamma_old = gamma; + double beta = new_gamma / gamma; + gamma = new_gamma; // d = g + d*beta; - scal(beta,d); + scal(beta, d); axpy(1.0, g, d); } // In-place recalculation Ad <- A*d to avoid re-allocating Ad - Ab.multiplyInPlace(d,Ad); + Ab.multiplyInPlace(d, Ad); + return false; } + }; + + /** + * conjugate gradient method. + * S: linear system, V: step vector, E: errors + */ + template + V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, + double epsilon_abs, size_t maxIterations, bool steepest = false) { + + CGState state(Ab, x, verbose, epsilon, epsilon_abs, maxIterations, steepest); + if (state.gamma < state.threshold) return x; + + if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = " + << maxIterations << ", ||g0||^2 = " << state.gamma + << ", threshold = " << state.threshold << endl; + + // loop maxIterations times + while (!state.step(Ab, x)) + ; + return x; } diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 653be27ae..0629b753c 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -26,6 +26,8 @@ using namespace std; using namespace gtsam; using namespace example; +static bool verbose = false; + /* ************************************************************************* */ TEST( Iterative, steepestDescent ) { @@ -38,7 +40,6 @@ TEST( Iterative, steepestDescent ) // Do gradient descent GaussianFactorGraph fg2 = createGaussianFactorGraph(); VectorConfig zero = createZeroDelta(); - bool verbose = false; VectorConfig actual = steepestDescent(fg2, zero, verbose); CHECK(assert_equal(expected,actual,1e-2)); } @@ -62,20 +63,20 @@ TEST( Iterative, conjugateGradientDescent ) // Do conjugate gradient descent, System version System Ab(A, b); - Vector actualX = conjugateGradientDescent(Ab, x0); + Vector actualX = conjugateGradientDescent(Ab, x0, verbose); CHECK(assert_equal(expectedX,actualX,1e-9)); // Do conjugate gradient descent, Matrix version - Vector actualX2 = conjugateGradientDescent(A, b, x0); + Vector actualX2 = conjugateGradientDescent(A, b, x0, verbose); CHECK(assert_equal(expectedX,actualX2,1e-9)); // Do conjugate gradient descent on factor graph VectorConfig zero = createZeroDelta(); - VectorConfig actual = conjugateGradientDescent(fg2, zero); + VectorConfig actual = conjugateGradientDescent(fg2, zero, verbose); CHECK(assert_equal(expected,actual,1e-2)); // Test method - VectorConfig actual2 = fg2.conjugateGradientDescent(zero); + VectorConfig actual2 = fg2.conjugateGradientDescent(zero, verbose); CHECK(assert_equal(expected,actual2,1e-2)); } @@ -122,7 +123,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) zeros.insert("x2",zero(3)); GaussianFactorGraph fg = graph.linearize(config); - VectorConfig actual = conjugateGradientDescent(fg, zeros, false, 1e-3, 1e-5, 100); + VectorConfig actual = conjugateGradientDescent(fg, zeros, verbose, 1e-3, 1e-5, 100); VectorConfig expected; expected.insert("x1", zero(3)); @@ -169,7 +170,7 @@ TEST( Iterative, subgraphPCG ) // Solve the subgraph PCG VectorConfig ybar = conjugateGradients (system, zeros, false, 1e-5, 1e-5, 100); + Errors> (system, zeros, verbose, 1e-5, 1e-5, 100); VectorConfig actual = system.x(ybar); VectorConfig expected; diff --git a/cpp/testSubgraphPreconditioner.cpp b/cpp/testSubgraphPreconditioner.cpp index 8139cb3b7..e5054ff4f 100644 --- a/cpp/testSubgraphPreconditioner.cpp +++ b/cpp/testSubgraphPreconditioner.cpp @@ -210,7 +210,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) // Compare with non preconditioned version: VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon, maxIterations); - CHECK(assert_equal(xtrue,actual2,1e-5)); + CHECK(assert_equal(xtrue,actual2,1e-4)); } /* ************************************************************************* */