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

move all implementation to cpp file

release/4.3a0
Varun Agrawal 2024-10-15 12:35:11 -04:00
parent 1de138678f
commit 69729d603b
2 changed files with 134 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -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>
@ -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();
@ -48,8 +48,7 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
new State(initialValues, graph.error(initialValues)))),
params_(params) {}
double NonlinearConjugateGradientOptimizer::error(
const Values& state) const {
double NonlinearConjugateGradientOptimizer::error(const Values& state) const {
return graph_.error(state);
}
@ -83,5 +82,129 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() {
return state_->values;
}
} /* namespace gtsam */
double NonlinearConjugateGradientOptimizer::lineSearch(
const Values& currentValues, const VectorValues& gradient) const {
/* 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);
Values newValues = advance(currentValues, newStep, gradient);
double newError = 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 Values testValues = advance(currentValues, testStep, gradient);
const double testError = 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;
}
std::tuple<Values, int>
NonlinearConjugateGradientOptimizer::nonlinearConjugateGradient(
const Values& initial, const NonlinearOptimizerParams& params,
const bool singleIteration, const bool gradientDescent) const {
size_t iteration = 0;
// 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};
}
Values currentValues = initial;
VectorValues currentGradient = gradient(currentValues), prevGradient,
direction = currentGradient;
/* do one step of gradient descent */
Values 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 << "Initial error: " << currentError << 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);
}
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 */

123
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -65,54 +65,8 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
const Values &optimize() override;
/** Implement the golden-section line search algorithm */
double lineSearch(const Values &currentValues, const VectorValues &gradient) const {
/* 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);
Values newValues = advance(currentValues, newStep, gradient);
double newError = 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 Values testValues = advance(currentValues, testStep, gradient);
const double testError = 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;
}
double lineSearch(const Values &currentValues,
const VectorValues &gradient) const;
/**
* Implement the nonlinear conjugate gradient method using the Polak-Ribiere
@ -124,78 +78,7 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
*/
std::tuple<Values, int> nonlinearConjugateGradient(
const Values &initial, const NonlinearOptimizerParams &params,
const bool singleIteration, const bool gradientDescent = false) const {
size_t iteration = 0;
// 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};
}
Values currentValues = initial;
VectorValues currentGradient = gradient(currentValues), prevGradient,
direction = currentGradient;
/* do one step of gradient descent */
Values 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 << "Initial error: " << currentError << 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);
}
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};
}
const bool singleIteration, const bool gradientDescent = false) const;
};
} // namespace gtsam