diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 107ec7d3f..6d7196ff5 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -1,7 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** - * @file GradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -16,36 +27,49 @@ using namespace std; namespace gtsam { -/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering - * Can be moved to NonlinearFactorGraph.h if desired */ -VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { +/** + * @brief Return the gradient vector of a nonlinear factor graph + * @param nfg the graph + * @param values a linearization point + * Can be moved to NonlinearFactorGraph.h if desired + */ +static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, + const Values &values) { // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); return linear->gradientAtZero(); } -double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { +double NonlinearConjugateGradientOptimizer::System::error( + const State &state) const { return graph_.error(state); } -NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { +NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient( + const State &state) const { return gradientInPlace(graph_, state); } -NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { + +NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance( + const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; step *= alpha; return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { - int dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_), state_.values, params_, true /* single iterations */); + int dummy; + boost::tie(state_.values, dummy) = nonlinearConjugateGradient( + System(graph_), state_.values, params_, true /* single iterations */); ++state_.iterations; state_.error = graph_.error(state_.values); } const Values& NonlinearConjugateGradientOptimizer::optimize() { - boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); + // Optimize until convergence + System system(graph_); + boost::tie(state_.values, state_.iterations) = // + nonlinearConjugateGradient(system, state_.values, params_, false); state_.error = graph_.error(state_.values); return state_.values; } diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 1ad8fa2ab..9c4db5fd9 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -1,8 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** - * @file GradientDescentOptimizer.cpp - * @brief + * @file NonlinearConjugateGradientOptimizer.h + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG * @author Yong-Dian Jian - * @date Jun 11, 2012 + * @date June 11, 2012 */ #pragma once @@ -13,15 +24,18 @@ namespace gtsam { -/** An implementation of the nonlinear cg method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState { +/** An implementation of the nonlinear CG method using the template below */ +class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState { public: typedef NonlinearOptimizerState Base; - NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values) - : Base(graph, values) {} + NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, + const Values& values) : + Base(graph, values) { + } }; -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer { + /* a class for the nonlinearConjugateGradient template */ class System { public: @@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz const NonlinearFactorGraph &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 ; + 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: + typedef NonlinearOptimizer Base; - typedef NonlinearConjugateGradientState States; typedef NonlinearOptimizerParams Parameters; typedef boost::shared_ptr shared_ptr; protected: - States state_; + NonlinearConjugateGradientState state_; Parameters params_; public: - NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params) {} + /// Constructor + NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Parameters& params = Parameters()) : + Base(graph), state_(graph, initialValues), params_(params) { + } + + /// Destructor + virtual ~NonlinearConjugateGradientOptimizer() { + } - virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); - virtual const Values& optimize (); - virtual const NonlinearOptimizerState& _state() const { return state_; } - virtual const NonlinearOptimizerParams& _params() const { return params_; } + virtual const Values& optimize(); + virtual const NonlinearOptimizerState& _state() const { + return state_; + } + virtual const NonlinearOptimizerParams& _params() const { + return params_; + } }; /** 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 */ @@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { // 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) ; + 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 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::fabs(testStep) + std::fabs(newStep)) ) { - return 0.5*(minStep+maxStep); + if ((maxStep - minStep) + < tau * (std::fabs(testStep) + std::fabs(newStep))) { + return 0.5 * (minStep + maxStep); } const V testValues = system.advance(currentValues, testStep, gradient); const double testError = system.error(testValues); // update the working range - if ( testError >= newError ) { - if ( flag ) maxStep = testStep; - else minStep = testStep; - } - else { - if ( flag ) { + if (testError >= newError) { + if (flag) + maxStep = testStep; + else + minStep = testStep; + } else { + if (flag) { minStep = newStep; newStep = testStep; newError = testError; - } - else { + } else { maxStep = newStep; newStep = testStep; newError = testError; @@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } /** - * Implement the nonlinear conjugate gradient method using the Polak-Ribieve 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 @@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { * * The last parameter is a switch between gradient-descent and conjugate gradient */ -template -boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { +template +boost::tuple nonlinearConjugateGradient(const S &system, + const V &initial, const NonlinearOptimizerParams ¶ms, + const bool singleIteration, const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V); @@ -129,57 +160,67 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initia // check if we're already close enough double currentError = system.error(initial); - if(currentError <= params.errorTol) { - if (params.verbosity >= NonlinearOptimizerParams::ERROR){ - std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl; + if (currentError <= params.errorTol) { + if (params.verbosity >= NonlinearOptimizerParams::ERROR) { + std::cout << "Exiting, as error = " << currentError << " < " + << params.errorTol << std::endl; } return boost::tie(initial, iteration); } V currentValues = initial; - typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient, - direction = currentGradient; + typename S::Gradient currentGradient = system.gradient(currentValues), + prevGradient, direction = currentGradient; /* do one step of gradient descent */ - V prevValues = currentValues; double prevError = currentError; + V prevValues = currentValues; + double prevError = currentError; double alpha = lineSearch(system, currentValues, direction); currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "Initial error: " << currentError << std::endl; // Iterative loop do { - if ( gradientDescent == true) { + if (gradientDescent == true) { direction = system.gradient(currentValues); - } - else { + } else { prevGradient = currentGradient; currentGradient = system.gradient(currentValues); - const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient)); - direction = currentGradient + (beta*direction); + // 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) + / currentGradient.dot(currentGradient)); + direction = currentGradient + (beta * direction); } alpha = lineSearch(system, currentValues, direction); - prevValues = currentValues; prevError = currentError; + prevValues = currentValues; + prevError = currentError; currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl; - } while( ++iteration < params.maxIterations && - !singleIteration && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity)); + 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; + if (params.verbosity >= NonlinearOptimizerParams::ERROR + && iteration >= params.maxIterations) + std::cout + << "nonlinearConjugateGradient: Terminating because reached maximum iterations" + << std::endl; return boost::tie(currentValues, iteration); } -} +} // \ namespace gtsam diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 81bcac344..b2d41c9fe 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -1,7 +1,14 @@ +/** + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Test simple CG optimizer + * @author Yong-Dian Jian + * @date June 11, 2012 + */ + /** * @file testGradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @brief Small test of NonlinearConjugateGradientOptimizer + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -22,7 +29,7 @@ using namespace std; using namespace gtsam; - +// Generate a small PoseSLAM problem boost::tuple generateProblem() { // 1. Create graph container and add factors to it @@ -56,49 +63,23 @@ boost::tuple generateProblem() { } /* ************************************************************************* */ -TEST(optimize, GradientDescentOptimizer) { +TEST(NonlinearConjugateGradientOptimizer, Optimize) { NonlinearFactorGraph graph; Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); - // cout << "initial error = " << graph.error(initialEstimate) << endl ; + cout << "initial error = " << graph.error(initialEstimate) << endl ; - // Single Step Optimization using Levenberg-Marquardt NonlinearOptimizerParams param; param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; + param.verbosity = NonlinearOptimizerParams::ERROR; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); -// cout << "gd1 solver final error = " << graph.error(result) << endl; + cout << "cg final error = " << graph.error(result) << endl; - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); - - CHECK(1); -} - -/* ************************************************************************* */ -TEST(optimize, ConjugateGradientOptimizer) { - - NonlinearFactorGraph graph; - Values initialEstimate; - - boost::tie(graph, initialEstimate) = generateProblem(); -// cout << "initial error = " << graph.error(initialEstimate) << endl ; - - // Single Step Optimization using Levenberg-Marquardt - NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; - - NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); - Values result = optimizer.optimize(); -// cout << "cg final error = " << graph.error(result) << endl; - - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } /* ************************************************************************* */