From 2d4ee5057a9651bd3e9a3e392570ccb0d5d807ab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 18 Oct 2024 10:30:01 -0400 Subject: [PATCH 1/2] template the directional methods --- .../NonlinearConjugateGradientOptimizer.cpp | 40 ----------------- .../NonlinearConjugateGradientOptimizer.h | 44 +++++++++++++++---- 2 files changed, 35 insertions(+), 49 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 438eac7a8..8b8542779 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -28,46 +28,6 @@ namespace gtsam { typedef internal::NonlinearOptimizerState State; -/* ************************************************************************* */ -double FletcherReeves(const VectorValues& currentGradient, - const VectorValues& prevGradient) { - // Fletcher-Reeves: beta = g_n'*g_n/g_n-1'*g_n-1 - const double beta = std::max(0.0, currentGradient.dot(currentGradient) / - prevGradient.dot(prevGradient)); - return beta; -} - -/* ************************************************************************* */ -double PolakRibiere(const VectorValues& currentGradient, - const VectorValues& prevGradient) { - // Polak-Ribiere: beta = g_n'*(g_n-g_n-1)/g_n-1'*g_n-1 - const double beta = - std::max(0.0, currentGradient.dot(currentGradient - prevGradient) / - prevGradient.dot(prevGradient)); - return beta; -} - -/* ************************************************************************* */ -double HestenesStiefel(const VectorValues& currentGradient, - const VectorValues& prevGradient, - const VectorValues& direction) { - // Hestenes-Stiefel: beta = g_n'*(g_n-g_n-1)/(-s_n-1')*(g_n-g_n-1) - VectorValues d = currentGradient - prevGradient; - const double beta = std::max(0.0, currentGradient.dot(d) / -direction.dot(d)); - return beta; -} - -/* ************************************************************************* */ -double DaiYuan(const VectorValues& currentGradient, - const VectorValues& prevGradient, - const VectorValues& direction) { - // Dai-Yuan: beta = g_n'*g_n/(-s_n-1')*(g_n-g_n-1) - const double beta = - std::max(0.0, currentGradient.dot(currentGradient) / - -direction.dot(currentGradient - prevGradient)); - return beta; -} - /** * @brief Return the gradient vector of a nonlinear factor graph * @param nfg the graph diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 09b932d42..1aee33a72 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -24,22 +24,48 @@ namespace gtsam { /// Fletcher-Reeves formula for computing β, the direction of steepest descent. -double FletcherReeves(const VectorValues ¤tGradient, - const VectorValues &prevGradient); +template +double FletcherReeves(const Gradient ¤tGradient, + const Gradient &prevGradient) { + // Fletcher-Reeves: beta = g_n'*g_n/g_n-1'*g_n-1 + const double beta = + currentGradient.dot(currentGradient) / prevGradient.dot(prevGradient); + return beta; +} /// Polak-Ribiere formula for computing β, the direction of steepest descent. -double PolakRibiere(const VectorValues ¤tGradient, - const VectorValues &prevGradient); +template +double PolakRibiere(const Gradient ¤tGradient, + const Gradient &prevGradient) { + // Polak-Ribiere: beta = g_n'*(g_n-g_n-1)/g_n-1'*g_n-1 + const double beta = + std::max(0.0, currentGradient.dot(currentGradient - prevGradient) / + prevGradient.dot(prevGradient)); + return beta; +} /// The Hestenes-Stiefel formula for computing β, /// the direction of steepest descent. -double HestenesStiefel(const VectorValues ¤tGradient, - const VectorValues &prevGradient, - const VectorValues &direction); +template +double HestenesStiefel(const Gradient ¤tGradient, + const Gradient &prevGradient, + const Gradient &direction) { + // Hestenes-Stiefel: beta = g_n'*(g_n-g_n-1)/(-s_n-1')*(g_n-g_n-1) + VectorValues d = currentGradient - prevGradient; + const double beta = std::max(0.0, currentGradient.dot(d) / -direction.dot(d)); + return beta; +} /// The Dai-Yuan formula for computing β, the direction of steepest descent. -double DaiYuan(const VectorValues ¤tGradient, - const VectorValues &prevGradient, const VectorValues &direction); +template +double DaiYuan(const Gradient ¤tGradient, const Gradient &prevGradient, + const VectorValues &direction) { + // Dai-Yuan: beta = g_n'*g_n/(-s_n-1')*(g_n-g_n-1) + const double beta = + std::max(0.0, currentGradient.dot(currentGradient) / + -direction.dot(currentGradient - prevGradient)); + return beta; +} enum class DirectionMethod { FletcherReeves, From 07b11bc9f1d4050666d22f453a777a7f60b9d00c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 19 Oct 2024 17:01:38 -0400 Subject: [PATCH 2/2] add test for different nonlinear CG direction methods --- ...estNonlinearConjugateGradientOptimizer.cpp | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 36673c7a0..f216e4a8c 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -79,6 +79,49 @@ TEST(NonlinearConjugateGradientOptimizer, Optimize) { EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } +/* ************************************************************************* */ +/// Test different direction methods +TEST(NonlinearConjugateGradientOptimizer, DirectionMethods) { + const auto [graph, initialEstimate] = generateProblem(); + + NonlinearOptimizerParams param; + param.maxIterations = + 500; /* requires a larger number of iterations to converge */ + param.verbosity = NonlinearOptimizerParams::SILENT; + + // Fletcher-Reeves + { + NonlinearConjugateGradientOptimizer optimizer( + graph, initialEstimate, param, DirectionMethod::FletcherReeves); + Values result = optimizer.optimize(); + + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); + } + // Polak-Ribiere + { + NonlinearConjugateGradientOptimizer optimizer( + graph, initialEstimate, param, DirectionMethod::PolakRibiere); + Values result = optimizer.optimize(); + + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); + } + // Hestenes-Stiefel + { + NonlinearConjugateGradientOptimizer optimizer( + graph, initialEstimate, param, DirectionMethod::HestenesStiefel); + Values result = optimizer.optimize(); + + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); + } + // Dai-Yuan + { + NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param, + DirectionMethod::DaiYuan); + Values result = optimizer.optimize(); + + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); + } +} /* ************************************************************************* */ int main() { TestResult tr;