Skip to content

Commit

Permalink
Merge pull request #1877 from borglab/conjugate-gradient-system
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored Oct 16, 2024
2 parents f5f7590 + d883e16 commit 5786073
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 69 deletions.
17 changes: 9 additions & 8 deletions gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,11 @@
* @date Jun 11, 2012
*/

#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/internal/NonlinearOptimizerState.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/internal/NonlinearOptimizerState.h>

#include <cmath>

Expand All @@ -34,8 +34,8 @@ typedef internal::NonlinearOptimizerState State;
* @param values a linearization point
* Can be moved to NonlinearFactorGraph.h if desired
*/
static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg,
const Values &values) {
static VectorValues gradientInPlace(const NonlinearFactorGraph& nfg,
const Values& values) {
// Linearize graph
GaussianFactorGraph::shared_ptr linear = nfg.linearize(values);
return linear->gradientAtZero();
Expand Down Expand Up @@ -71,7 +71,8 @@ NonlinearConjugateGradientOptimizer::System::advance(const State& current,
GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
const auto [newValues, dummy] = nonlinearConjugateGradient<System, Values>(
System(graph_), 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.
return nullptr;
Expand All @@ -82,9 +83,9 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() {
System system(graph_);
const auto [newValues, iterations] =
nonlinearConjugateGradient(system, 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;
}

} /* namespace gtsam */

115 changes: 54 additions & 61 deletions gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,90 +24,80 @@
namespace gtsam {

/** 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 Values State;
typedef VectorValues Gradient;
typedef NonlinearOptimizerParams Parameters;

protected:
protected:
const NonlinearFactorGraph &graph_;

public:
System(const NonlinearFactorGraph &graph) :
graph_(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:

public:
typedef NonlinearOptimizer Base;
typedef NonlinearOptimizerParams Parameters;
typedef std::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;

protected:
Parameters params_;

const NonlinearOptimizerParams& _params() const override {
return params_;
}

public:
const NonlinearOptimizerParams &_params() const override { return params_; }

public:
/// Constructor
NonlinearConjugateGradientOptimizer(
const NonlinearFactorGraph &graph, const Values &initialValues,
const Parameters &params = Parameters());
NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph &graph,
const Values &initialValues,
const Parameters &params = Parameters());

/// Destructor
~NonlinearConjugateGradientOptimizer() override {
}
~NonlinearConjugateGradientOptimizer() override {}

/**
* Perform a single iteration, returning GaussianFactorGraph corresponding to
/**
* Perform a single iteration, returning GaussianFactorGraph corresponding to
* the linearized factor graph.
*/
GaussianFactorGraph::shared_ptr iterate() override;

/**
* Optimize for the maximum-likelihood estimate, returning a the optimized
/**
* Optimize for the maximum-likelihood estimate, returning a the optimized
* variable assignments.
*/
const Values& optimize() override;
const Values &optimize() override;
};

/** Implement the golden-section line search algorithm */
template<class S, class V, class W>
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);
// 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);
const double testStep = flag ? newStep + resphi * (maxStep - newStep)
: newStep - resphi * (newStep - minStep);

if ((maxStep - minStep)
< tau * (std::abs(testStep) + std::abs(newStep))) {
if ((maxStep - minStep) < tau * (std::abs(testStep) + std::abs(newStep))) {
return 0.5 * (minStep + maxStep);
}

Expand Down Expand Up @@ -136,19 +126,21 @@ 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.
*
* 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 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
* The last parameter is a switch between gradient-descent and conjugate
* gradient
*/
template<class S, class V>
std::tuple<V, int> nonlinearConjugateGradient(const S &system,
const V &initial, const NonlinearOptimizerParams &params,
template <class S, class V>
std::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;
Expand All @@ -158,7 +150,7 @@ std::tuple<V, int> nonlinearConjugateGradient(const S &system,
if (currentError <= params.errorTol) {
if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
std::cout << "Exiting, as error = " << currentError << " < "
<< params.errorTol << std::endl;
<< params.errorTol << std::endl;
}
return {initial, iteration};
}
Expand Down Expand Up @@ -186,9 +178,9 @@ std::tuple<V, int> nonlinearConjugateGradient(const S &system,
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));
const double beta =
std::max(0.0, currentGradient.dot(currentGradient - prevGradient) /
prevGradient.dot(prevGradient));
direction = currentGradient + (beta * direction);
}

Expand All @@ -206,20 +198,21 @@ std::tuple<V, int> nonlinearConjugateGradient(const S &system,

// 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));
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 {currentValues, iteration};
}

} // \ namespace gtsam

} // namespace gtsam

0 comments on commit 5786073

Please sign in to comment.