Loading...
Searching...
No Matches
NonlinearConjugateGradientOptimizer.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <boost/tuple/tuple.hpp>
namespace gtsam {
class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
/* a class for the nonlinearConjugateGradient template */
class System {
public:
typedef Values State;
typedef VectorValues Gradient;
typedef NonlinearOptimizerParams Parameters;
protected:
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 &current, const double alpha,
const Gradient &g) const;
};
public:
typedef NonlinearOptimizer Base;
typedef NonlinearOptimizerParams Parameters;
typedef boost::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
protected:
Parameters params_;
const NonlinearOptimizerParams& _params() const override {
return params_;
}
public:
NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph,
const Values& initialValues, const Parameters& params = Parameters());
~NonlinearConjugateGradientOptimizer() override {
}
GaussianFactorGraph::shared_ptr iterate() override;
const Values& optimize() override;
};
template<class S, class V, class W>
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);
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);
if ((maxStep - minStep)
< tau * (std::abs(testStep) + std::abs(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) {
minStep = newStep;
newStep = testStep;
newError = testError;
} else {
maxStep = newStep;
newStep = testStep;
newError = testError;
}
}
}
return 0.0;
}
template<class S, class V>
boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
const V &initial, const NonlinearOptimizerParams &params,
const bool singleIteration, const bool gradientDescent = false) {
// GTSAM_CONCEPT_MANIFOLD_TYPE(V)
size_t iteration = 0;
// 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 boost::tie(initial, iteration);
}
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);
// Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::ERROR)
std::cout << "Initial error: " << currentError << std::endl;
// Iterative loop
do {
if (gradientDescent == true) {
direction = system.gradient(currentValues);
} else {
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));
direction = currentGradient + (beta * direction);
}
alpha = lineSearch(system, currentValues, direction);
prevValues = currentValues;
prevError = currentError;
currentValues = system.advance(prevValues, alpha, direction);
currentError = system.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 boost::tie(currentValues, iteration);
}
} // \ namespace gtsam
Manifold.h
Base class and basic functions for Manifold types.
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::lineSearch
double lineSearch(const S &system, const V currentValues, const W &gradient)
Implement the golden-section line search algorithm.
Definition NonlinearConjugateGradientOptimizer.h:88
gtsam::checkConvergence
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decr...
Definition NonlinearOptimizer.cpp:185
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
gtsam::nonlinearConjugateGradient
boost::tuple< V, int > nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const bool gradientDescent=false)
Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http:/...
Definition NonlinearConjugateGradientOptimizer.h:148
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::NonlinearConjugateGradientOptimizer::~NonlinearConjugateGradientOptimizer
~NonlinearConjugateGradientOptimizer() override
Destructor.
Definition NonlinearConjugateGradientOptimizer.h:70
gtsam::NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer
NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const Parameters &params=Parameters())
Constructor.
Definition NonlinearConjugateGradientOptimizer.cpp:44
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::NonlinearOptimizer::graph
const NonlinearFactorGraph & graph() const
return the graph with nonlinear factors
Definition NonlinearOptimizer.h:118
gtsam::NonlinearOptimizer::graph_
NonlinearFactorGraph graph_
The graph with nonlinear factors.
Definition NonlinearOptimizer.h:78
gtsam::NonlinearOptimizer::error
double error() const
return error in current optimizer state
Definition NonlinearOptimizer.cpp:52
gtsam::NonlinearOptimizer::NonlinearOptimizer
NonlinearOptimizer(const NonlinearFactorGraph &graph, std::unique_ptr< internal::NonlinearOptimizerState > state)
Constructor for initial construction of base classes.
Definition NonlinearOptimizer.cpp:44
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
gtsam::NonlinearOptimizerParams::absoluteErrorTol
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5).
Definition NonlinearOptimizerParams.h:43
gtsam::NonlinearOptimizerParams::iterationHook
IterationHook iterationHook
Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
Definition NonlinearOptimizerParams.h:94
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100).
Definition NonlinearOptimizerParams.h:41
gtsam::NonlinearOptimizerParams::verbosity
Verbosity verbosity
The printing verbosity during optimization (default SILENT).
Definition NonlinearOptimizerParams.h:45
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5).
Definition NonlinearOptimizerParams.h:42
gtsam::NonlinearOptimizerParams::errorTol
double errorTol
The maximum total error to stop iterating (default 0.0).
Definition NonlinearOptimizerParams.h:44
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65