55 lines
1.8 KiB
C++
55 lines
1.8 KiB
C++
/**
|
|
* @file GradientDescentOptimizer.cpp
|
|
* @brief
|
|
* @author ydjian
|
|
* @date Jun 11, 2012
|
|
*/
|
|
|
|
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
|
|
#include <gtsam/nonlinear/Values.h>
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam/linear/VectorValues.h>
|
|
|
|
#include <cmath>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering
|
|
* Can be moved to NonlinearFactorGraph.h if desired */
|
|
VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) {
|
|
// Linearize graph
|
|
GaussianFactorGraph::shared_ptr linear = nfg.linearize(values);
|
|
return linear->gradientAtZero();
|
|
}
|
|
|
|
double NonlinearConjugateGradientOptimizer::System::error(const State &state) const {
|
|
return graph_.error(state);
|
|
}
|
|
|
|
NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const {
|
|
return gradientInPlace(graph_, state);
|
|
}
|
|
NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const {
|
|
Gradient step = g;
|
|
step *= alpha;
|
|
return current.retract(step);
|
|
}
|
|
|
|
void NonlinearConjugateGradientOptimizer::iterate() {
|
|
int dummy ;
|
|
boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>(System(graph_), state_.values, params_, true /* single iterations */);
|
|
++state_.iterations;
|
|
state_.error = graph_.error(state_.values);
|
|
}
|
|
|
|
const Values& NonlinearConjugateGradientOptimizer::optimize() {
|
|
boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient<System, Values>(System(graph_), state_.values, params_, false /* up to convergent */);
|
|
state_.error = graph_.error(state_.values);
|
|
return state_.values;
|
|
}
|
|
|
|
} /* namespace gtsam */
|
|
|