remove System inside NonlinearConjugateGradientOptimizer
Varun Agrawal
parent
a94169a973
commit
ba6e2b8d7f
|
|
@ -48,26 +48,27 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
|
|||
new State(initialValues, graph.error(initialValues)))),
|
||||
params_(params) {}
|
||||
|
||||
double NonlinearConjugateGradientOptimizer::System::error(
|
||||
double NonlinearConjugateGradientOptimizer::error(
|
||||
const Values& state) const {
|
||||
return graph_.error(state);
|
||||
}
|
||||
|
||||
VectorValues NonlinearConjugateGradientOptimizer::System::gradient(
|
||||
VectorValues NonlinearConjugateGradientOptimizer::gradient(
|
||||
const Values& state) const {
|
||||
return gradientInPlace(graph_, state);
|
||||
}
|
||||
|
||||
Values NonlinearConjugateGradientOptimizer::System::advance(
|
||||
Values NonlinearConjugateGradientOptimizer::advance(
|
||||
const Values& current, const double alpha,
|
||||
const VectorValues& gradient) const {
|
||||
return current.retract(alpha * gradient);
|
||||
}
|
||||
|
||||
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));
|
||||
const auto [newValues, dummy] = nonlinearConjugateGradient<Values>(
|
||||
state_->values, params_, true /* single iteration */);
|
||||
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;
|
||||
|
|
@ -75,10 +76,10 @@ GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
|
|||
|
||||
const Values& NonlinearConjugateGradientOptimizer::optimize() {
|
||||
// Optimize until convergence
|
||||
System system(graph_);
|
||||
const auto [newValues, iterations] =
|
||||
nonlinearConjugateGradient(system, state_->values, params_, false);
|
||||
state_.reset(new State(std::move(newValues), graph_.error(newValues), iterations));
|
||||
nonlinearConjugateGradient(state_->values, params_, false);
|
||||
state_.reset(
|
||||
new State(std::move(newValues), graph_.error(newValues), iterations));
|
||||
return state_->values;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -24,28 +24,9 @@
|
|||
namespace gtsam {
|
||||
|
||||
/** An implementation of the nonlinear CG method using the template below */
|
||||
class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
|
||||
|
||||
/* a class for the nonlinearConjugateGradient template */
|
||||
class System {
|
||||
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 ¤t, const double alpha,
|
||||
const VectorValues &g) const;
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
|
||||
: public NonlinearOptimizer {
|
||||
public:
|
||||
typedef NonlinearOptimizer Base;
|
||||
typedef NonlinearOptimizerParams Parameters;
|
||||
typedef std::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
|
||||
|
|
@ -53,20 +34,23 @@ public:
|
|||
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 ¶ms = Parameters());
|
||||
NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph &graph,
|
||||
const Values &initialValues,
|
||||
const Parameters ¶ms = Parameters());
|
||||
|
||||
/// Destructor
|
||||
~NonlinearConjugateGradientOptimizer() override {
|
||||
}
|
||||
~NonlinearConjugateGradientOptimizer() override {}
|
||||
|
||||
double error(const Values &state) const;
|
||||
|
||||
VectorValues gradient(const Values &state) const;
|
||||
|
||||
Values advance(const Values ¤t, const double alpha,
|
||||
const VectorValues &g) const;
|
||||
|
||||
/**
|
||||
* Perform a single iteration, returning GaussianFactorGraph corresponding to
|
||||
|
|
@ -79,145 +63,147 @@ public:
|
|||
* variable assignments.
|
||||
*/
|
||||
const Values& optimize() override;
|
||||
};
|
||||
|
||||
/** Implement the golden-section line search algorithm */
|
||||
template<class S, class V, class W>
|
||||
double lineSearch(const S &system, const V currentValues, const W &gradient) {
|
||||
/** Implement the golden-section line search algorithm */
|
||||
template <class V, class W>
|
||||
double lineSearch(const V currentValues, const W &gradient) {
|
||||
/* normalize it such that it becomes a unit vector */
|
||||
const double g = gradient.norm();
|
||||
|
||||
/* 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 = advance(currentValues, newStep, gradient);
|
||||
double newError = error(newValues);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
if ((maxStep - minStep)
|
||||
< tau * (std::abs(testStep) + std::abs(newStep))) {
|
||||
return 0.5 * (minStep + maxStep);
|
||||
}
|
||||
const V testValues = advance(currentValues, testStep, gradient);
|
||||
const double testError = error(testValues);
|
||||
|
||||
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;
|
||||
// update the working range
|
||||
if (testError >= newError) {
|
||||
if (flag)
|
||||
maxStep = testStep;
|
||||
else
|
||||
minStep = testStep;
|
||||
} else {
|
||||
maxStep = newStep;
|
||||
newStep = testStep;
|
||||
newError = testError;
|
||||
if (flag) {
|
||||
minStep = newStep;
|
||||
newStep = testStep;
|
||||
newError = testError;
|
||||
} else {
|
||||
maxStep = newStep;
|
||||
newStep = testStep;
|
||||
newError = testError;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 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 ¶ms,
|
||||
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 {initial, iteration};
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
V currentValues = initial;
|
||||
VectorValues currentGradient = system.gradient(currentValues), prevGradient,
|
||||
direction = currentGradient;
|
||||
/**
|
||||
* Implement the nonlinear conjugate gradient method using the Polak-Ribiere
|
||||
* formula suggested in
|
||||
* http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
|
||||
*
|
||||
* The V class
|
||||
* denotes the state or the solution.
|
||||
*
|
||||
* The last parameter is a switch between gradient-descent and conjugate
|
||||
* gradient
|
||||
*/
|
||||
template <class V>
|
||||
std::tuple<V, int> nonlinearConjugateGradient(
|
||||
const V &initial, const NonlinearOptimizerParams ¶ms,
|
||||
const bool singleIteration, const bool gradientDescent = false) {
|
||||
// GTSAM_CONCEPT_MANIFOLD_TYPE(V)
|
||||
|
||||
/* 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);
|
||||
size_t iteration = 0;
|
||||
|
||||
// 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);
|
||||
// check if we're already close enough
|
||||
double currentError = error(initial);
|
||||
if (currentError <= params.errorTol) {
|
||||
if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
|
||||
std::cout << "Exiting, as error = " << currentError << " < "
|
||||
<< params.errorTol << std::endl;
|
||||
}
|
||||
return {initial, iteration};
|
||||
}
|
||||
|
||||
alpha = lineSearch(system, currentValues, direction);
|
||||
V currentValues = initial;
|
||||
VectorValues currentGradient = gradient(currentValues), prevGradient,
|
||||
direction = currentGradient;
|
||||
|
||||
prevValues = currentValues;
|
||||
prevError = currentError;
|
||||
|
||||
currentValues = system.advance(prevValues, alpha, direction);
|
||||
currentError = system.error(currentValues);
|
||||
|
||||
// User hook:
|
||||
if (params.iterationHook)
|
||||
params.iterationHook(iteration, prevError, currentError);
|
||||
/* do one step of gradient descent */
|
||||
V prevValues = currentValues;
|
||||
double prevError = currentError;
|
||||
double alpha = lineSearch(currentValues, direction);
|
||||
currentValues = advance(prevValues, alpha, direction);
|
||||
currentError = error(currentValues);
|
||||
|
||||
// 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 << "Initial error: " << currentError << std::endl;
|
||||
|
||||
// Printing if verbose
|
||||
if (params.verbosity >= NonlinearOptimizerParams::ERROR
|
||||
&& iteration >= params.maxIterations)
|
||||
std::cout
|
||||
<< "nonlinearConjugateGradient: Terminating because reached maximum iterations"
|
||||
<< std::endl;
|
||||
// Iterative loop
|
||||
do {
|
||||
if (gradientDescent == true) {
|
||||
direction = gradient(currentValues);
|
||||
} else {
|
||||
prevGradient = currentGradient;
|
||||
currentGradient = 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);
|
||||
}
|
||||
|
||||
return {currentValues, iteration};
|
||||
}
|
||||
alpha = lineSearch(currentValues, direction);
|
||||
|
||||
prevValues = currentValues;
|
||||
prevError = currentError;
|
||||
|
||||
currentValues = advance(prevValues, alpha, direction);
|
||||
currentError = 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 {currentValues, iteration};
|
||||
}
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue