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

add a generic nonlinear cg solver

release/4.3a0
Yong-Dian Jian 2012-06-13 01:21:10 +00:00
parent b8ca742d3d
commit cee80c1938
5 changed files with 124 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/linear/VectorValues.cpp
View File

@ -151,6 +151,14 @@ VectorValues VectorValues::operator+(const VectorValues& c) const {
return result;
}
/* ************************************************************************* */
VectorValues VectorValues::operator-(const VectorValues& c) const {
assert(this->hasSameStructure(c));
VectorValues result(SameStructure(c));
result.values_ = this->values_ - c.values_;
return result;
}
/* ************************************************************************* */
void VectorValues::operator+=(const VectorValues& c) {
assert(this->hasSameStructure(c));

16
gtsam/linear/VectorValues.h
View File

@ -273,6 +273,12 @@ namespace gtsam {
*/
VectorValues operator+(const VectorValues& c) const;
/**
* + operator does element-wise subtraction. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
VectorValues operator-(const VectorValues& c) const;
/**
* += operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
@ -301,6 +307,16 @@ namespace gtsam {
void copyStructureFrom(const VectorValues& other);
public:
/**
* scale a vector by a scalar
*/
friend VectorValues operator*(const double a, const VectorValues &V) {
VectorValues result(VectorValues::SameStructure(V));
result.values_ = a * V.values_;
return result;
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend size_t dim(const VectorValues& V) {
return V.dim();

24
gtsam/nonlinear/GradientDescentOptimizer.cpp
View File

@ -85,7 +85,11 @@ void GradientDescentOptimizer::iterate() {
const double testError = graph_.error(testValues);
// update the working range
if ( testError < newError ) {
if ( testError >= newError ) {
if ( flag ) maxStep = testStep;
else minStep = testStep;
}
else {
if ( flag ) {
minStep = newStep;
newStep = testStep;
@ -97,14 +101,6 @@ void GradientDescentOptimizer::iterate() {
newError = testError;
}
}
else {
if ( flag ) {
maxStep = testStep;
}
else {
minStep = testStep;
}
}
if ( nloVerbosity ) {
std::cout << "minStep = " << minStep << ", maxStep = " << maxStep << ", newStep = " << newStep << ", newError = " << newError << std::endl;
@ -114,23 +110,23 @@ void GradientDescentOptimizer::iterate() {
++state_.iterations;
}
double GradientDescentOptimizer2::System::error(const State &state) const {
double ConjugateGradientOptimizer::System::error(const State &state) const {
return graph_.error(state);
}
GradientDescentOptimizer2::System::Gradient GradientDescentOptimizer2::System::gradient(const State &state) const {
ConjugateGradientOptimizer::System::Gradient ConjugateGradientOptimizer::System::gradient(const State &state) const {
Gradient result = state.zeroVectors(ordering_);
gradientInPlace(graph_, state, ordering_, result);
return result;
}
GradientDescentOptimizer2::System::State GradientDescentOptimizer2::System::advance(const State &current, const double alpha, const Gradient &g) const {
ConjugateGradientOptimizer::System::State ConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const {
Gradient step = g;
step.vector() *= alpha;
return current.retract(step, ordering_);
}
Values GradientDescentOptimizer2::optimize() {
return gradientDescent<System, Values>(System(graph_, *ordering_), initialEstimate_, params_);
Values ConjugateGradientOptimizer::optimize() {
return conjugateGradient<System, Values>(System(graph_, *ordering_), initialEstimate_, params_, !cg_);
}
} /* namespace gtsam */

100
gtsam/nonlinear/GradientDescentOptimizer.h
View File

@ -13,19 +13,6 @@ namespace gtsam {
/* an implementation of gradient-descent method using the NLO interface */
class GradientDescentParams : public NonlinearOptimizerParams {
public:
typedef NonlinearOptimizerParams Base;
GradientDescentParams():Base() {}
virtual void print(const std::string& str = "") const {
Base::print(str);
}
virtual ~GradientDescentParams() {}
};
class GradientDescentState : public NonlinearOptimizerState {
public:
@ -43,7 +30,7 @@ public:
typedef boost::shared_ptr<GradientDescentOptimizer> shared_ptr;
typedef NonlinearOptimizer Base;
typedef GradientDescentState States;
typedef GradientDescentParams Parameters;
typedef NonlinearOptimizerParams Parameters;
protected:
@ -70,15 +57,11 @@ protected:
};
#include <gtsam/linear/IterativeSolver.h>
/**
* An implementation of the nonlinear cg method using the template below
*/
/* Yet another implementation of the gradient-descent method using the iterative.h interface */
class GradientDescentParams2 : public NonlinearOptimizerParams {
public:
GradientDescentParams2(){}
};
class GradientDescentOptimizer2 {
class ConjugateGradientOptimizer {
class System {
@ -102,8 +85,8 @@ class GradientDescentOptimizer2 {
public:
typedef GradientDescentParams2 Parameters;
typedef boost::shared_ptr<GradientDescentOptimizer2> shared_ptr;
typedef NonlinearOptimizerParams Parameters;
typedef boost::shared_ptr<ConjugateGradientOptimizer> shared_ptr;
protected:
@ -112,20 +95,25 @@ protected:
Parameters params_;
Ordering::shared_ptr ordering_;
VectorValues::shared_ptr gradient_;
bool cg_;
public:
GradientDescentOptimizer2(const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params = Parameters())
ConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
const Parameters& params = Parameters(), const bool cg = true)
: graph_(graph), initialEstimate_(initialValues), params_(params),
ordering_(initialValues.orderingArbitrary()),
gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))) {}
gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))),
cg_(cg) {}
virtual ~GradientDescentOptimizer2() {}
virtual ~ConjugateGradientOptimizer() {}
virtual Values optimize () ;
};
/**
* 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) {
@ -171,37 +159,62 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
}
}
}
return 0.0;
}
template <class S, class V>
V gradientDescent(const S &system, const V &initial, const NonlinearOptimizerParams &params) {
/**
* Implement the nonlinear conjugate gradient method using the Polak-Ribieve 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
*/
V currentValues = initial, prevValues;
double currentError = system.error(currentValues), prevError;
Index iteration = 0;
template <class S, class V>
V conjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool gradientDescent) {
// 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 currentValues;
return initial;
}
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);
Index iteration = 0;
// Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl;
// Iterative loop
do {
// Do next iteration
const typename S::Gradient gradient = system.gradient(currentValues);
const double alpha = lineSearch(system, currentValues, gradient);
prevValues = currentValues;
prevError = currentError;
if ( gradientDescent == true) {
direction = system.gradient(currentValues);
}
else {
prevGradient = currentGradient;
currentGradient = system.gradient(currentValues);
const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient));
direction = currentGradient + (beta*direction);
}
currentValues = system.advance(prevValues, alpha, gradient);
alpha = lineSearch(system, currentValues, direction);
prevValues = currentValues; prevError = currentError;
currentValues = system.advance(prevValues, alpha, direction);
currentError = system.error(currentValues);
// Maybe show output
@ -217,4 +230,9 @@ V gradientDescent(const S &system, const V &initial, const NonlinearOptimizerPar
return currentValues;
}
}

36
tests/testGradientDescentOptimizer.cpp
View File

@ -62,13 +62,13 @@ TEST(optimize, GradientDescentOptimizer) {
// cout << "initial error = " << graph.error(initialEstimate) << endl ;
// Single Step Optimization using Levenberg-Marquardt
GradientDescentParams param;
NonlinearOptimizerParams param;
param.maxIterations = 500; /* requires a larger number of iterations to converge */
param.verbosity = NonlinearOptimizerParams::SILENT;
GradientDescentOptimizer optimizer(graph, initialEstimate, param);
Values result = optimizer.optimize();
// cout << "solver final error = " << graph.error(result) << endl;
// cout << "gd1 solver final error = " << graph.error(result) << endl;
/* the optimality of the solution is not comparable to the */
DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
@ -76,6 +76,29 @@ TEST(optimize, GradientDescentOptimizer) {
CHECK(1);
}
/* ************************************************************************* */
TEST(optimize, ConjugateGradientOptimizer) {
pose2SLAM::Graph graph ;
pose2SLAM::Values initialEstimate;
boost::tie(graph, initialEstimate) = generateProblem();
// cout << "initial error = " << graph.error(initialEstimate) << endl ;
// Single Step Optimization using Levenberg-Marquardt
NonlinearOptimizerParams param;
param.maxIterations = 500; /* requires a larger number of iterations to converge */
param.verbosity = NonlinearOptimizerParams::SILENT;
ConjugateGradientOptimizer optimizer(graph, initialEstimate, param, true);
Values result = optimizer.optimize();
// cout << "cg final error = " << graph.error(result) << endl;
/* the optimality of the solution is not comparable to the */
DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
}
/* ************************************************************************* */
TEST(optimize, GradientDescentOptimizer2) {
@ -86,19 +109,22 @@ TEST(optimize, GradientDescentOptimizer2) {
// cout << "initial error = " << graph.error(initialEstimate) << endl ;
// Single Step Optimization using Levenberg-Marquardt
GradientDescentParams2 param;
NonlinearOptimizerParams param;
param.maxIterations = 500; /* requires a larger number of iterations to converge */
param.verbosity = NonlinearOptimizerParams::SILENT;
GradientDescentOptimizer2 optimizer(graph, initialEstimate, param);
ConjugateGradientOptimizer optimizer(graph, initialEstimate, param, false);
Values result = optimizer.optimize();
// cout << "solver final error = " << graph.error(result) << endl;
// cout << "gd2 solver final error = " << graph.error(result) << endl;
/* the optimality of the solution is not comparable to the */
DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */