diff --git a/gtsam/nonlinear/GradientDescentOptimizer.cpp b/gtsam/nonlinear/GradientDescentOptimizer.cpp new file mode 100644 index 000000000..9416b4f6c --- /dev/null +++ b/gtsam/nonlinear/GradientDescentOptimizer.cpp @@ -0,0 +1,120 @@ +/** + * @file GradientDescentOptimizer.cpp + * @brief + * @author ydjian + * @date Jun 11, 2012 + */ + +#include + +#include +#include +#include +#include +#include + +#include + +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 + */ +void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, const Ordering &ordering, VectorValues &g) { + + // Linearize graph + GaussianFactorGraph::shared_ptr linear = nfg.linearize(values, ordering); + const FactorGraph::shared_ptr jfg = linear->dynamicCastFactors >(); + + // compute the gradient direction + gtsam::gradientAtZero(*jfg, g); +} + + +/* ************************************************************************* */ +void GradientDescentOptimizer::iterate() { + + + // Pull out parameters we'll use + const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; + + // compute the gradient vector + gradientInPlace(graph_, state_.values, *ordering_, *gradient_); + + /* normalize it such that it becomes a unit vector */ + const double g = gradient_->vector().norm(); + gradient_->vector() /= g; + + // perform the golden section search algorithm to decide the the optimal step size + // detail refer to http://en.wikipedia.org/wiki/Golden_section_search + VectorValues step = VectorValues::SameStructure(*gradient_); + const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5; + double minStep = -1.0, maxStep = 0, + newStep = minStep + (maxStep-minStep) / (phi+1.0) ; + + step.vector() = newStep * gradient_->vector(); + Values newValues = state_.values.retract(step, *ordering_); + double newError = graph_.error(newValues); + + if ( nloVerbosity ) { + std::cout << "minStep = " << minStep << ", maxStep = " << maxStep << ", newStep = " << newStep << ", newError = " << newError << std::endl; + } + + 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::fabs(testStep) + std::fabs(newStep)) ) { + newStep = 0.5*(minStep+maxStep); + step.vector() = newStep * gradient_->vector(); + newValues = state_.values.retract(step, *ordering_); + newError = graph_.error(newValues); + + if ( newError < state_.error ) { + state_.values = state_.values.retract(step, *ordering_); + state_.error = graph_.error(state_.values); + } + + break; + } + + step.vector() = testStep * gradient_->vector(); + const Values testValues = state_.values.retract(step, *ordering_); + const double testError = graph_.error(testValues); + + // update the working range + if ( testError < newError ) { + if ( flag ) { + minStep = newStep; + newStep = testStep; + newError = testError; + } + else { + maxStep = newStep; + newStep = testStep; + newError = testError; + } + } + else { + if ( flag ) { + maxStep = testStep; + } + else { + minStep = testStep; + } + } + + if ( nloVerbosity ) { + std::cout << "minStep = " << minStep << ", maxStep = " << maxStep << ", newStep = " << newStep << ", newError = " << newError << std::endl; + } + } + + // Increment the iteration counter + ++state_.iterations; +} + +} /* namespace gtsam */ diff --git a/gtsam/nonlinear/GradientDescentOptimizer.h b/gtsam/nonlinear/GradientDescentOptimizer.h new file mode 100644 index 000000000..03e1c018b --- /dev/null +++ b/gtsam/nonlinear/GradientDescentOptimizer.h @@ -0,0 +1,74 @@ +/** + * @file GradientDescentOptimizer.cpp + * @brief + * @author ydjian + * @date Jun 11, 2012 + */ + +#pragma once + +#include + +namespace gtsam { + +class GradientDescentOptimizer; + +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: + + typedef NonlinearOptimizerState Base; + + GradientDescentState(const NonlinearFactorGraph& graph, const Values& values) + : Base(graph, values) {} +}; + +class GradientDescentOptimizer : public NonlinearOptimizer { + +public: + + typedef boost::shared_ptr shared_ptr; + typedef NonlinearOptimizer Base; + typedef GradientDescentState States; + typedef GradientDescentParams Parameters; + +protected: + + Parameters params_; + States state_; + Ordering::shared_ptr ordering_; + VectorValues::shared_ptr gradient_; + +public: + + GradientDescentOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params = Parameters()) + : Base(graph), params_(params), state_(graph, initialValues), + ordering_(initialValues.orderingArbitrary()), + gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))) {} + + virtual ~GradientDescentOptimizer() {} + + virtual void iterate(); + +protected: + + virtual const NonlinearOptimizerState& _state() const { return state_; } + virtual const NonlinearOptimizerParams& _params() const { return params_; } +}; + + + +} diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp new file mode 100644 index 000000000..de4de4a6f --- /dev/null +++ b/tests/testGradientDescentOptimizer.cpp @@ -0,0 +1,66 @@ +/** + * @file testGradientDescentOptimizer.cpp + * @brief + * @author ydjian + * @date Jun 11, 2012 + */ + +#include +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(optimize, GradientDescentOptimizer) { + + // 1. Create graph container and add factors to it + pose2SLAM::Graph graph ; + + // 2a. Add Gaussian prior + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); + graph.addPrior(1, priorMean, priorNoise); + + // 2b. Add odometry factors + SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); + graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + + // 2c. Add pose constraint + SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1)); + graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + + // 3. Create the data structure to hold the initialEstimate estinmate to the solution + pose2SLAM::Values initialEstimate; + Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); + Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3); + Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4); + Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5); + // cout << "initial error = " << graph.error(initialEstimate) << endl ; + + // 4. Single Step Optimization using Levenberg-Marquardt + GradientDescentParams 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; + + /* 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); } +/* ************************************************************************* */