From 69729d603beb76e300a9ea40d9109af456b1a5f5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Oct 2024 12:35:11 -0400 Subject: [PATCH] move all implementation to cpp file --- .../NonlinearConjugateGradientOptimizer.cpp | 139 +++++++++++++++++- .../NonlinearConjugateGradientOptimizer.h | 123 +--------------- 2 files changed, 134 insertions(+), 128 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 6190b041e..9fce1776b 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -16,11 +16,11 @@ * @date Jun 11, 2012 */ -#include -#include -#include #include #include +#include +#include +#include #include @@ -34,8 +34,8 @@ typedef internal::NonlinearOptimizerState State; * @param values a linearization point * Can be moved to NonlinearFactorGraph.h if desired */ -static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, - const Values &values) { +static VectorValues gradientInPlace(const NonlinearFactorGraph& nfg, + const Values& values) { // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); return linear->gradientAtZero(); @@ -48,8 +48,7 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer( new State(initialValues, graph.error(initialValues)))), params_(params) {} -double NonlinearConjugateGradientOptimizer::error( - const Values& state) const { +double NonlinearConjugateGradientOptimizer::error(const Values& state) const { return graph_.error(state); } @@ -83,5 +82,129 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() { return state_->values; } -} /* namespace gtsam */ +double NonlinearConjugateGradientOptimizer::lineSearch( + const Values& currentValues, const VectorValues& gradient) const { + /* normalize it such that it becomes a unit vector */ + const double g = gradient.norm(); + // perform the golden section search algorithm to decide the the optimal + // step size detail refer to + // http://en.wikipedia.org/wiki/Golden_section_search + const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, + tau = 1e-5; + double minStep = -1.0 / g, maxStep = 0, + newStep = minStep + (maxStep - minStep) / (phi + 1.0); + + Values newValues = advance(currentValues, newStep, gradient); + double newError = error(newValues); + + while (true) { + const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; + const double testStep = flag ? newStep + resphi * (maxStep - newStep) + : newStep - resphi * (newStep - minStep); + + if ((maxStep - minStep) < tau * (std::abs(testStep) + std::abs(newStep))) { + return 0.5 * (minStep + maxStep); + } + + const Values testValues = advance(currentValues, testStep, gradient); + const double testError = error(testValues); + + // update the working range + if (testError >= newError) { + if (flag) + maxStep = testStep; + else + minStep = testStep; + } else { + if (flag) { + minStep = newStep; + newStep = testStep; + newError = testError; + } else { + maxStep = newStep; + newStep = testStep; + newError = testError; + } + } + } + return 0.0; +} + +std::tuple +NonlinearConjugateGradientOptimizer::nonlinearConjugateGradient( + const Values& initial, const NonlinearOptimizerParams& params, + const bool singleIteration, const bool gradientDescent) const { + size_t iteration = 0; + + // check if we're already close enough + double currentError = error(initial); + if (currentError <= params.errorTol) { + if (params.verbosity >= NonlinearOptimizerParams::ERROR) { + std::cout << "Exiting, as error = " << currentError << " < " + << params.errorTol << std::endl; + } + return {initial, iteration}; + } + + Values currentValues = initial; + VectorValues currentGradient = gradient(currentValues), prevGradient, + direction = currentGradient; + + /* do one step of gradient descent */ + Values prevValues = currentValues; + double prevError = currentError; + double alpha = lineSearch(currentValues, direction); + currentValues = advance(prevValues, alpha, direction); + currentError = error(currentValues); + + // Maybe show output + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "Initial error: " << currentError << std::endl; + + // Iterative loop + do { + if (gradientDescent == true) { + direction = gradient(currentValues); + } else { + prevGradient = currentGradient; + currentGradient = gradient(currentValues); + // Polak-Ribiere: beta = g'*(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)); + direction = currentGradient + (beta * direction); + } + + alpha = lineSearch(currentValues, direction); + + prevValues = currentValues; + prevError = currentError; + + currentValues = advance(prevValues, alpha, direction); + currentError = error(currentValues); + + // User hook: + if (params.iterationHook) + params.iterationHook(iteration, prevError, currentError); + + // Maybe show output + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "iteration: " << iteration + << ", currentError: " << currentError << std::endl; + } while (++iteration < params.maxIterations && !singleIteration && + !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, + params.errorTol, prevError, currentError, + params.verbosity)); + + // Printing if verbose + if (params.verbosity >= NonlinearOptimizerParams::ERROR && + iteration >= params.maxIterations) + std::cout << "nonlinearConjugateGradient: Terminating because reached " + "maximum iterations" + << std::endl; + + return {currentValues, iteration}; +} + +} /* namespace gtsam */ diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 28de45894..cdc0634d6 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -65,54 +65,8 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer const Values &optimize() override; /** Implement the golden-section line search algorithm */ - double lineSearch(const Values ¤tValues, const VectorValues &gradient) const { - /* normalize it such that it becomes a unit vector */ - const double g = gradient.norm(); - - // perform the golden section search algorithm to decide the the optimal - // step size detail refer to - // http://en.wikipedia.org/wiki/Golden_section_search - const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, - tau = 1e-5; - double minStep = -1.0 / g, maxStep = 0, - newStep = minStep + (maxStep - minStep) / (phi + 1.0); - - Values newValues = advance(currentValues, newStep, gradient); - double newError = error(newValues); - - while (true) { - const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; - const double testStep = flag ? newStep + resphi * (maxStep - newStep) - : newStep - resphi * (newStep - minStep); - - if ((maxStep - minStep) < - tau * (std::abs(testStep) + std::abs(newStep))) { - return 0.5 * (minStep + maxStep); - } - - const Values testValues = advance(currentValues, testStep, gradient); - const double testError = error(testValues); - - // update the working range - if (testError >= newError) { - if (flag) - maxStep = testStep; - else - minStep = testStep; - } else { - if (flag) { - minStep = newStep; - newStep = testStep; - newError = testError; - } else { - maxStep = newStep; - newStep = testStep; - newError = testError; - } - } - } - return 0.0; - } + double lineSearch(const Values ¤tValues, + const VectorValues &gradient) const; /** * Implement the nonlinear conjugate gradient method using the Polak-Ribiere @@ -124,78 +78,7 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer */ std::tuple nonlinearConjugateGradient( const Values &initial, const NonlinearOptimizerParams ¶ms, - const bool singleIteration, const bool gradientDescent = false) const { - size_t iteration = 0; - - // check if we're already close enough - double currentError = error(initial); - if (currentError <= params.errorTol) { - if (params.verbosity >= NonlinearOptimizerParams::ERROR) { - std::cout << "Exiting, as error = " << currentError << " < " - << params.errorTol << std::endl; - } - return {initial, iteration}; - } - - Values currentValues = initial; - VectorValues currentGradient = gradient(currentValues), prevGradient, - direction = currentGradient; - - /* do one step of gradient descent */ - Values prevValues = currentValues; - double prevError = currentError; - double alpha = lineSearch(currentValues, direction); - currentValues = advance(prevValues, alpha, direction); - currentError = error(currentValues); - - // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) - std::cout << "Initial error: " << currentError << std::endl; - - // Iterative loop - do { - if (gradientDescent == true) { - direction = gradient(currentValues); - } else { - prevGradient = currentGradient; - currentGradient = gradient(currentValues); - // Polak-Ribiere: beta = g'*(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)); - direction = currentGradient + (beta * direction); - } - - alpha = lineSearch(currentValues, direction); - - prevValues = currentValues; - prevError = currentError; - - currentValues = advance(prevValues, alpha, direction); - currentError = error(currentValues); - - // User hook: - if (params.iterationHook) - params.iterationHook(iteration, prevError, currentError); - - // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) - std::cout << "iteration: " << iteration - << ", currentError: " << currentError << std::endl; - } while (++iteration < params.maxIterations && !singleIteration && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, - params.errorTol, prevError, currentError, - params.verbosity)); - - // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::ERROR && - iteration >= params.maxIterations) - std::cout << "nonlinearConjugateGradient: Terminating because reached " - "maximum iterations" - << std::endl; - - return {currentValues, iteration}; - } + const bool singleIteration, const bool gradientDescent = false) const; }; } // namespace gtsam