12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove System inside NonlinearConjugateGradientOptimizer

release/4.3a0
Varun Agrawal 2024-10-15 10:06:37 -04:00
parent a94169a973
commit ba6e2b8d7f
2 changed files with 145 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -48,26 +48,27 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
new State(initialValues, graph.error(initialValues)))), new State(initialValues, graph.error(initialValues)))),
params_(params) {} params_(params) {}
double NonlinearConjugateGradientOptimizer::System::error( double NonlinearConjugateGradientOptimizer::error(
const Values& state) const { const Values& state) const {
return graph_.error(state); return graph_.error(state);
} }
VectorValues NonlinearConjugateGradientOptimizer::System::gradient( VectorValues NonlinearConjugateGradientOptimizer::gradient(
const Values& state) const { const Values& state) const {
return gradientInPlace(graph_, state); return gradientInPlace(graph_, state);
} }
Values NonlinearConjugateGradientOptimizer::System::advance( Values NonlinearConjugateGradientOptimizer::advance(
const Values& current, const double alpha, const Values& current, const double alpha,
const VectorValues& gradient) const { const VectorValues& gradient) const {
return current.retract(alpha * gradient); return current.retract(alpha * gradient);
} }
GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() { GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
const auto [newValues, dummy] = nonlinearConjugateGradient<System, Values>( const auto [newValues, dummy] = nonlinearConjugateGradient<Values>(
System(graph_), state_->values, params_, true /* single iteration */); 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. // NOTE(frank): We don't linearize this system, so we must return null here.
return nullptr; return nullptr;
@ -75,10 +76,10 @@ GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
const Values& NonlinearConjugateGradientOptimizer::optimize() { const Values& NonlinearConjugateGradientOptimizer::optimize() {
// Optimize until convergence // Optimize until convergence
System system(graph_);
const auto [newValues, iterations] = const auto [newValues, iterations] =
nonlinearConjugateGradient(system, state_->values, params_, false); nonlinearConjugateGradient(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; return state_->values;
} }

138
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -24,28 +24,9 @@
namespace gtsam { namespace gtsam {
/** An implementation of the nonlinear CG method using the template below */ /** 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 NonlinearOptimizerParams Parameters;
protected:
const NonlinearFactorGraph &graph_;
public:
System(const NonlinearFactorGraph &graph) :
graph_(graph) {
}
double error(const Values &state) const;
VectorValues gradient(const Values &state) const;
Values advance(const Values &current, const double alpha,
const VectorValues &g) const;
};
public:
typedef NonlinearOptimizer Base; typedef NonlinearOptimizer Base;
typedef NonlinearOptimizerParams Parameters; typedef NonlinearOptimizerParams Parameters;
typedef std::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr; typedef std::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
@ -53,20 +34,23 @@ public:
protected: protected:
Parameters params_; Parameters params_;
const NonlinearOptimizerParams& _params() const override { const NonlinearOptimizerParams &_params() const override { return params_; }
return params_;
}
public: public:
/// Constructor /// Constructor
NonlinearConjugateGradientOptimizer( NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph &graph,
const NonlinearFactorGraph &graph, const Values &initialValues, const Values &initialValues,
const Parameters &params = Parameters()); const Parameters &params = Parameters());
/// Destructor /// Destructor
~NonlinearConjugateGradientOptimizer() override { ~NonlinearConjugateGradientOptimizer() override {}
}
double error(const Values &state) const;
VectorValues gradient(const Values &state) const;
Values advance(const Values &current, const double alpha,
const VectorValues &g) const;
/** /**
* Perform a single iteration, returning GaussianFactorGraph corresponding to * Perform a single iteration, returning GaussianFactorGraph corresponding to
@ -79,38 +63,36 @@ public:
* variable assignments. * variable assignments.
*/ */
const Values& optimize() override; const Values& optimize() override;
};
/** Implement the golden-section line search algorithm */ /** Implement the golden-section line search algorithm */
template<class S, class V, class W> template <class V, class W>
double lineSearch(const S &system, const V currentValues, const W &gradient) { double lineSearch(const V currentValues, const W &gradient) {
/* normalize it such that it becomes a unit vector */ /* normalize it such that it becomes a unit vector */
const double g = gradient.norm(); const double g = gradient.norm();
// perform the golden section search algorithm to decide the the optimal step size // perform the golden section search algorithm to decide the the optimal
// detail refer to http://en.wikipedia.org/wiki/Golden_section_search // step size detail refer to
const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = // http://en.wikipedia.org/wiki/Golden_section_search
1e-5; const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi,
double minStep = -1.0 / g, maxStep = 0, newStep = minStep tau = 1e-5;
+ (maxStep - minStep) / (phi + 1.0); double minStep = -1.0 / g, maxStep = 0,
newStep = minStep + (maxStep - minStep) / (phi + 1.0);
V newValues = system.advance(currentValues, newStep, gradient); V newValues = advance(currentValues, newStep, gradient);
double newError = system.error(newValues); double newError = error(newValues);
while (true) { while (true) {
const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
const double testStep = const double testStep = flag ? newStep + resphi * (maxStep - newStep)
flag ? newStep + resphi * (maxStep - newStep) : : newStep - resphi * (newStep - minStep);
newStep - resphi * (newStep - minStep);
if ((maxStep - minStep) if ((maxStep - minStep) <
< tau * (std::abs(testStep) + std::abs(newStep))) { tau * (std::abs(testStep) + std::abs(newStep))) {
return 0.5 * (minStep + maxStep); return 0.5 * (minStep + maxStep);
} }
const V testValues = system.advance(currentValues, testStep, gradient); const V testValues = advance(currentValues, testStep, gradient);
const double testError = system.error(testValues); const double testError = error(testValues);
// update the working range // update the working range
if (testError >= newError) { if (testError >= newError) {
@ -134,25 +116,26 @@ 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. * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
* *
* The S (system) class requires three member functions: error(state), gradient(state) and * The V class
* advance(state, step-size, direction). The V class denotes the state or the solution. * 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<class S, class V> template <class V>
std::tuple<V, int> nonlinearConjugateGradient(const S &system, std::tuple<V, int> nonlinearConjugateGradient(
const V &initial, const NonlinearOptimizerParams &params, const V &initial, const NonlinearOptimizerParams &params,
const bool singleIteration, const bool gradientDescent = false) { const bool singleIteration, const bool gradientDescent = false) {
// GTSAM_CONCEPT_MANIFOLD_TYPE(V) // GTSAM_CONCEPT_MANIFOLD_TYPE(V)
size_t iteration = 0; size_t iteration = 0;
// check if we're already close enough // check if we're already close enough
double currentError = system.error(initial); double currentError = error(initial);
if (currentError <= params.errorTol) { if (currentError <= params.errorTol) {
if (params.verbosity >= NonlinearOptimizerParams::ERROR) { if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
std::cout << "Exiting, as error = " << currentError << " < " std::cout << "Exiting, as error = " << currentError << " < "
@ -162,15 +145,15 @@ std::tuple<V, int> nonlinearConjugateGradient(const S &system,
} }
V currentValues = initial; V currentValues = initial;
VectorValues currentGradient = system.gradient(currentValues), prevGradient, VectorValues currentGradient = gradient(currentValues), prevGradient,
direction = currentGradient; direction = currentGradient;
/* do one step of gradient descent */ /* do one step of gradient descent */
V prevValues = currentValues; V prevValues = currentValues;
double prevError = currentError; double prevError = currentError;
double alpha = lineSearch(system, currentValues, direction); double alpha = lineSearch(currentValues, direction);
currentValues = system.advance(prevValues, alpha, direction); currentValues = advance(prevValues, alpha, direction);
currentError = system.error(currentValues); currentError = error(currentValues);
// Maybe show output // Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::ERROR) if (params.verbosity >= NonlinearOptimizerParams::ERROR)
@ -179,24 +162,24 @@ std::tuple<V, int> nonlinearConjugateGradient(const S &system,
// Iterative loop // Iterative loop
do { do {
if (gradientDescent == true) { if (gradientDescent == true) {
direction = system.gradient(currentValues); direction = gradient(currentValues);
} else { } else {
prevGradient = currentGradient; prevGradient = currentGradient;
currentGradient = system.gradient(currentValues); currentGradient = gradient(currentValues);
// Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1
const double beta = std::max(0.0, const double beta =
currentGradient.dot(currentGradient - prevGradient) std::max(0.0, currentGradient.dot(currentGradient - prevGradient) /
/ prevGradient.dot(prevGradient)); prevGradient.dot(prevGradient));
direction = currentGradient + (beta * direction); direction = currentGradient + (beta * direction);
} }
alpha = lineSearch(system, currentValues, direction); alpha = lineSearch(currentValues, direction);
prevValues = currentValues; prevValues = currentValues;
prevError = currentError; prevError = currentError;
currentValues = system.advance(prevValues, alpha, direction); currentValues = advance(prevValues, alpha, direction);
currentError = system.error(currentValues); currentError = error(currentValues);
// User hook: // User hook:
if (params.iterationHook) if (params.iterationHook)
@ -204,20 +187,23 @@ std::tuple<V, int> nonlinearConjugateGradient(const S &system,
// Maybe show output // Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::ERROR) if (params.verbosity >= NonlinearOptimizerParams::ERROR)
std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; std::cout << "iteration: " << iteration
} while (++iteration < params.maxIterations && !singleIteration << ", currentError: " << currentError << std::endl;
&& !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, } while (++iteration < params.maxIterations && !singleIteration &&
params.errorTol, prevError, currentError, params.verbosity)); !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
params.errorTol, prevError, currentError,
params.verbosity));
// Printing if verbose // Printing if verbose
if (params.verbosity >= NonlinearOptimizerParams::ERROR if (params.verbosity >= NonlinearOptimizerParams::ERROR &&
&& iteration >= params.maxIterations) iteration >= params.maxIterations)
std::cout std::cout << "nonlinearConjugateGradient: Terminating because reached "
<< "nonlinearConjugateGradient: Terminating because reached maximum iterations" "maximum iterations"
<< std::endl; << std::endl;
return {currentValues, iteration}; return {currentValues, iteration};
} }
};
} // \ namespace gtsam } // \ namespace gtsam