diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index a0af31c70..5868d8452 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -151,6 +151,14 @@ VectorValues VectorValues::operator+(const VectorValues& c) const { return result; } +/* ************************************************************************* */ +VectorValues VectorValues::operator-(const VectorValues& c) const { + assert(this->hasSameStructure(c)); + VectorValues result(SameStructure(c)); + result.values_ = this->values_ - c.values_; + return result; +} + /* ************************************************************************* */ void VectorValues::operator+=(const VectorValues& c) { assert(this->hasSameStructure(c)); diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 3531989c9..0a32e6812 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -273,6 +273,12 @@ namespace gtsam { */ VectorValues operator+(const VectorValues& c) const; + /** + * + operator does element-wise subtraction. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). + */ + VectorValues operator-(const VectorValues& c) const; + /** * += operator does element-wise addition. Both VectorValues must have the * same structure (checked when NDEBUG is not defined). @@ -301,6 +307,16 @@ namespace gtsam { void copyStructureFrom(const VectorValues& other); public: + + /** + * scale a vector by a scalar + */ + friend VectorValues operator*(const double a, const VectorValues &V) { + VectorValues result(VectorValues::SameStructure(V)); + result.values_ = a * V.values_; + return result; + } + /// TODO: linear algebra interface seems to have been added for SPCG. friend size_t dim(const VectorValues& V) { return V.dim(); diff --git a/gtsam/nonlinear/GradientDescentOptimizer.cpp b/gtsam/nonlinear/GradientDescentOptimizer.cpp index 9a4b6b37b..2243577c7 100644 --- a/gtsam/nonlinear/GradientDescentOptimizer.cpp +++ b/gtsam/nonlinear/GradientDescentOptimizer.cpp @@ -85,7 +85,11 @@ void GradientDescentOptimizer::iterate() { const double testError = graph_.error(testValues); // update the working range - if ( testError < newError ) { + if ( testError >= newError ) { + if ( flag ) maxStep = testStep; + else minStep = testStep; + } + else { if ( flag ) { minStep = newStep; newStep = testStep; @@ -97,14 +101,6 @@ void GradientDescentOptimizer::iterate() { newError = testError; } } - else { - if ( flag ) { - maxStep = testStep; - } - else { - minStep = testStep; - } - } if ( nloVerbosity ) { std::cout << "minStep = " << minStep << ", maxStep = " << maxStep << ", newStep = " << newStep << ", newError = " << newError << std::endl; @@ -114,23 +110,23 @@ void GradientDescentOptimizer::iterate() { ++state_.iterations; } -double GradientDescentOptimizer2::System::error(const State &state) const { +double ConjugateGradientOptimizer::System::error(const State &state) const { return graph_.error(state); } -GradientDescentOptimizer2::System::Gradient GradientDescentOptimizer2::System::gradient(const State &state) const { +ConjugateGradientOptimizer::System::Gradient ConjugateGradientOptimizer::System::gradient(const State &state) const { Gradient result = state.zeroVectors(ordering_); gradientInPlace(graph_, state, ordering_, result); return result; } -GradientDescentOptimizer2::System::State GradientDescentOptimizer2::System::advance(const State ¤t, const double alpha, const Gradient &g) const { +ConjugateGradientOptimizer::System::State ConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; step.vector() *= alpha; return current.retract(step, ordering_); } -Values GradientDescentOptimizer2::optimize() { - return gradientDescent(System(graph_, *ordering_), initialEstimate_, params_); +Values ConjugateGradientOptimizer::optimize() { + return conjugateGradient(System(graph_, *ordering_), initialEstimate_, params_, !cg_); } } /* namespace gtsam */ diff --git a/gtsam/nonlinear/GradientDescentOptimizer.h b/gtsam/nonlinear/GradientDescentOptimizer.h index 1a78416de..6f79b4779 100644 --- a/gtsam/nonlinear/GradientDescentOptimizer.h +++ b/gtsam/nonlinear/GradientDescentOptimizer.h @@ -13,19 +13,6 @@ namespace gtsam { /* an implementation of gradient-descent method using the NLO interface */ -class GradientDescentParams : public NonlinearOptimizerParams { - -public: - typedef NonlinearOptimizerParams Base; - - GradientDescentParams():Base() {} - virtual void print(const std::string& str = "") const { - Base::print(str); - } - - virtual ~GradientDescentParams() {} -}; - class GradientDescentState : public NonlinearOptimizerState { public: @@ -43,7 +30,7 @@ public: typedef boost::shared_ptr shared_ptr; typedef NonlinearOptimizer Base; typedef GradientDescentState States; - typedef GradientDescentParams Parameters; + typedef NonlinearOptimizerParams Parameters; protected: @@ -70,15 +57,11 @@ protected: }; -#include +/** + * An implementation of the nonlinear cg method using the template below + */ -/* Yet another implementation of the gradient-descent method using the iterative.h interface */ -class GradientDescentParams2 : public NonlinearOptimizerParams { -public: - GradientDescentParams2(){} -}; - -class GradientDescentOptimizer2 { +class ConjugateGradientOptimizer { class System { @@ -102,8 +85,8 @@ class GradientDescentOptimizer2 { public: - typedef GradientDescentParams2 Parameters; - typedef boost::shared_ptr shared_ptr; + typedef NonlinearOptimizerParams Parameters; + typedef boost::shared_ptr shared_ptr; protected: @@ -112,20 +95,25 @@ protected: Parameters params_; Ordering::shared_ptr ordering_; VectorValues::shared_ptr gradient_; + bool cg_; public: - GradientDescentOptimizer2(const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params = Parameters()) + ConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const Parameters& params = Parameters(), const bool cg = true) : graph_(graph), initialEstimate_(initialValues), params_(params), ordering_(initialValues.orderingArbitrary()), - gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))) {} + gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))), + cg_(cg) {} - virtual ~GradientDescentOptimizer2() {} + virtual ~ConjugateGradientOptimizer() {} virtual Values optimize () ; }; - +/** + * Implement the golden-section line search algorithm + */ template double lineSearch(const S &system, const V currentValues, const W &gradient) { @@ -171,37 +159,62 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } } } - return 0.0; } -template -V gradientDescent(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms) { +/** + * Implement the nonlinear conjugate gradient method using the Polak-Ribieve 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 last parameter is a switch between gradient-descent and conjugate gradient + */ - V currentValues = initial, prevValues; - double currentError = system.error(currentValues), prevError; - Index iteration = 0; +template +V conjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool gradientDescent) { // 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; - return currentValues; + return initial; } + V currentValues = initial; + typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient, + direction = currentGradient; + + /* do one step of gradient descent */ + V prevValues = currentValues; double prevError = currentError; + double alpha = lineSearch(system, currentValues, direction); + currentValues = system.advance(prevValues, alpha, direction); + currentError = system.error(currentValues); + Index iteration = 0; + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl; // Iterative loop do { - // Do next iteration - const typename S::Gradient gradient = system.gradient(currentValues); - const double alpha = lineSearch(system, currentValues, gradient); - prevValues = currentValues; - prevError = currentError; + if ( gradientDescent == true) { + direction = system.gradient(currentValues); + } + 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); + } - currentValues = system.advance(prevValues, alpha, gradient); + alpha = lineSearch(system, currentValues, direction); + + prevValues = currentValues; prevError = currentError; + + currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output @@ -217,4 +230,9 @@ V gradientDescent(const S &system, const V &initial, const NonlinearOptimizerPar return currentValues; } + + + + + } diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index cfd7db3d7..54130b664 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -62,13 +62,13 @@ TEST(optimize, GradientDescentOptimizer) { // cout << "initial error = " << graph.error(initialEstimate) << endl ; // Single Step Optimization using Levenberg-Marquardt - GradientDescentParams param; + NonlinearOptimizerParams param; param.maxIterations = 500; /* requires a larger number of iterations to converge */ param.verbosity = NonlinearOptimizerParams::SILENT; GradientDescentOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); - // cout << "solver final error = " << graph.error(result) << endl; +// cout << "gd1 solver 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); @@ -76,6 +76,29 @@ TEST(optimize, GradientDescentOptimizer) { CHECK(1); } +/* ************************************************************************* */ +TEST(optimize, ConjugateGradientOptimizer) { + + pose2SLAM::Graph graph ; + pose2SLAM::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; + + + ConjugateGradientOptimizer optimizer(graph, initialEstimate, param, true); + 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); +} + /* ************************************************************************* */ TEST(optimize, GradientDescentOptimizer2) { @@ -86,19 +109,22 @@ TEST(optimize, GradientDescentOptimizer2) { // cout << "initial error = " << graph.error(initialEstimate) << endl ; // Single Step Optimization using Levenberg-Marquardt - GradientDescentParams2 param; + NonlinearOptimizerParams param; param.maxIterations = 500; /* requires a larger number of iterations to converge */ param.verbosity = NonlinearOptimizerParams::SILENT; - GradientDescentOptimizer2 optimizer(graph, initialEstimate, param); + ConjugateGradientOptimizer optimizer(graph, initialEstimate, param, false); Values result = optimizer.optimize(); -// cout << "solver final error = " << graph.error(result) << endl; +// cout << "gd2 solver 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); } + + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */