diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 211acc78d..0ab56f6d1 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(); @@ -71,7 +71,8 @@ NonlinearConjugateGradientOptimizer::System::advance(const State& current, GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() { const auto [newValues, dummy] = nonlinearConjugateGradient( System(graph_), state_->values, params_, true /* single iteration */); - state_.reset(new State(newValues, graph_.error(newValues), state_->iterations + 1)); + state_.reset( + new State(newValues, graph_.error(newValues), state_->iterations + 1)); // NOTE(frank): We don't linearize this system, so we must return null here. return nullptr; @@ -82,9 +83,9 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() { System system(graph_); const auto [newValues, iterations] = nonlinearConjugateGradient(system, state_->values, params_, false); - state_.reset(new State(std::move(newValues), graph_.error(newValues), iterations)); + state_.reset( + new State(std::move(newValues), graph_.error(newValues), iterations)); return state_->values; } } /* namespace gtsam */ - diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index f662403dc..369291fae 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -24,30 +24,27 @@ namespace gtsam { /** An implementation of the nonlinear CG method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { - +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer + : public NonlinearOptimizer { /* a class for the nonlinearConjugateGradient template */ class System { - public: + public: typedef Values State; typedef VectorValues Gradient; typedef NonlinearOptimizerParams Parameters; - protected: + protected: const NonlinearFactorGraph &graph_; - public: - System(const NonlinearFactorGraph &graph) : - graph_(graph) { - } + public: + System(const NonlinearFactorGraph &graph) : graph_(graph) {} double error(const State &state) const; Gradient gradient(const State &state) const; State advance(const State ¤t, const double alpha, const Gradient &g) const; }; -public: - + public: typedef NonlinearOptimizer Base; typedef NonlinearOptimizerParams Parameters; typedef std::shared_ptr shared_ptr; @@ -55,59 +52,52 @@ public: protected: Parameters params_; - const NonlinearOptimizerParams& _params() const override { - return params_; - } - -public: + const NonlinearOptimizerParams &_params() const override { return params_; } + public: /// Constructor - NonlinearConjugateGradientOptimizer( - const NonlinearFactorGraph &graph, const Values &initialValues, - const Parameters ¶ms = Parameters()); + NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph &graph, + const Values &initialValues, + const Parameters ¶ms = Parameters()); /// Destructor - ~NonlinearConjugateGradientOptimizer() override { - } + ~NonlinearConjugateGradientOptimizer() override {} - /** - * Perform a single iteration, returning GaussianFactorGraph corresponding to + /** + * Perform a single iteration, returning GaussianFactorGraph corresponding to * the linearized factor graph. */ GaussianFactorGraph::shared_ptr iterate() override; - /** - * Optimize for the maximum-likelihood estimate, returning a the optimized + /** + * Optimize for the maximum-likelihood estimate, returning a the optimized * variable assignments. */ - const Values& optimize() override; + const Values &optimize() override; }; /** Implement the golden-section line search algorithm */ -template +template double lineSearch(const S &system, const V currentValues, const W &gradient) { - /* 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); + // 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); V newValues = system.advance(currentValues, newStep, gradient); double newError = system.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); + const double testStep = flag ? newStep + resphi * (maxStep - newStep) + : newStep - resphi * (newStep - minStep); - if ((maxStep - minStep) - < tau * (std::abs(testStep) + std::abs(newStep))) { + if ((maxStep - minStep) < tau * (std::abs(testStep) + std::abs(newStep))) { return 0.5 * (minStep + maxStep); } @@ -136,19 +126,21 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } /** - * Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in + * Implement the nonlinear conjugate gradient method using the Polak-Ribiere + * formula suggested in * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method. * - * The S (system) class requires three member functions: error(state), gradient(state) and - * advance(state, step-size, direction). The V class denotes the state or the solution. + * The S (system) class requires three member functions: error(state), + * gradient(state) and advance(state, step-size, direction). The V class denotes + * the state or the solution. * - * The last parameter is a switch between gradient-descent and conjugate gradient + * The last parameter is a switch between gradient-descent and conjugate + * gradient */ -template -std::tuple nonlinearConjugateGradient(const S &system, - const V &initial, const NonlinearOptimizerParams ¶ms, +template +std::tuple nonlinearConjugateGradient( + const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { - // GTSAM_CONCEPT_MANIFOLD_TYPE(V) size_t iteration = 0; @@ -158,7 +150,7 @@ std::tuple nonlinearConjugateGradient(const S &system, if (currentError <= params.errorTol) { if (params.verbosity >= NonlinearOptimizerParams::ERROR) { std::cout << "Exiting, as error = " << currentError << " < " - << params.errorTol << std::endl; + << params.errorTol << std::endl; } return {initial, iteration}; } @@ -186,9 +178,9 @@ std::tuple nonlinearConjugateGradient(const S &system, prevGradient = currentGradient; currentGradient = system.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)); + const double beta = + std::max(0.0, currentGradient.dot(currentGradient - prevGradient) / + prevGradient.dot(prevGradient)); direction = currentGradient + (beta * direction); } @@ -206,20 +198,21 @@ std::tuple nonlinearConjugateGradient(const S &system, // 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)); + 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; + if (params.verbosity >= NonlinearOptimizerParams::ERROR && + iteration >= params.maxIterations) + std::cout << "nonlinearConjugateGradient: Terminating because reached " + "maximum iterations" + << std::endl; return {currentValues, iteration}; } -} // \ namespace gtsam - +} // namespace gtsam