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

Made NonlinearConjugateGradientOptimizer compile

release/4.3a0
Richard Roberts 2013-08-05 22:31:22 +00:00
parent 55ce9eed1d
commit 90b1349f23
3 changed files with 20 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -5,13 +5,10 @@
* @date Jun 11, 2012 * @date Jun 11, 2012
*/ */
#if 0
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h> #include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValuesOrdered.h> #include <gtsam/linear/VectorValues.h>
#include <cmath> #include <cmath>
@ -21,18 +18,10 @@ namespace gtsam {
/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering /* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering
* Can be moved to NonlinearFactorGraph.h if desired */ * Can be moved to NonlinearFactorGraph.h if desired */
void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, const OrderingOrdered &ordering, VectorValuesOrdered &g) { VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) {
// Linearize graph // Linearize graph
GaussianFactorGraphOrdered::shared_ptr linear = nfg.linearize(values, ordering); GaussianFactorGraph::shared_ptr linear = nfg.linearize(values);
FactorGraphOrdered<JacobianFactorOrdered> jfg; jfg.reserve(linear->size()); return linear->gradientAtZero();
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, *linear) {
if(boost::shared_ptr<JacobianFactorOrdered> jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
jfg.push_back((jf));
else
jfg.push_back(boost::make_shared<JacobianFactorOrdered>(*factor));
}
// compute the gradient direction
gradientAtZero(jfg, g);
} }
double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { double NonlinearConjugateGradientOptimizer::System::error(const State &state) const {
@ -40,29 +29,26 @@ double NonlinearConjugateGradientOptimizer::System::error(const State &state) co
} }
NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const {
Gradient result = state.zeroVectors(ordering_); return gradientInPlace(graph_, state);
gradientInPlace(graph_, state, ordering_, result);
return result;
} }
NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const { NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const {
Gradient step = g; Gradient step = g;
scal(alpha, step); step *= alpha;
return current.retract(step, ordering_); return current.retract(step);
} }
void NonlinearConjugateGradientOptimizer::iterate() { void NonlinearConjugateGradientOptimizer::iterate() {
size_t dummy ; size_t dummy ;
boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>(System(graph_, *ordering_), state_.values, params_, true /* single iterations */); boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>(System(graph_), state_.values, params_, true /* single iterations */);
++state_.iterations; ++state_.iterations;
state_.error = graph_.error(state_.values); state_.error = graph_.error(state_.values);
} }
const Values& NonlinearConjugateGradientOptimizer::optimize() { const Values& NonlinearConjugateGradientOptimizer::optimize() {
boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient<System, Values>(System(graph_, *ordering_), state_.values, params_, false /* up to convergent */); boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient<System, Values>(System(graph_), state_.values, params_, false /* up to convergent */);
state_.error = graph_.error(state_.values); state_.error = graph_.error(state_.values);
return state_.values; return state_.values;
} }
} /* namespace gtsam */ } /* namespace gtsam */
#endif

13
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -7,8 +7,6 @@
#pragma once #pragma once
#if 0
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
@ -28,15 +26,14 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz
class System { class System {
public: public:
typedef Values State; typedef Values State;
typedef VectorValuesOrdered Gradient; typedef VectorValues Gradient;
typedef NonlinearOptimizerParams Parameters; typedef NonlinearOptimizerParams Parameters;
protected: protected:
const NonlinearFactorGraph &graph_; const NonlinearFactorGraph &graph_;
const OrderingOrdered &ordering_;
public: public:
System(const NonlinearFactorGraph &graph, const OrderingOrdered &ordering): graph_(graph), ordering_(ordering) {} System(const NonlinearFactorGraph &graph): graph_(graph) {}
double error(const State &state) const ; double error(const State &state) const ;
Gradient gradient(const State &state) const ; Gradient gradient(const State &state) const ;
State advance(const State &current, const double alpha, const Gradient &g) const ; State advance(const State &current, const double alpha, const Gradient &g) const ;
@ -51,15 +48,12 @@ public:
protected: protected:
States state_; States state_;
Parameters params_; Parameters params_;
OrderingOrdered::shared_ptr ordering_;
VectorValuesOrdered::shared_ptr gradient_;
public: public:
NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
const Parameters& params = Parameters()) const Parameters& params = Parameters())
: Base(graph), state_(graph, initialValues), params_(params), ordering_(initialValues.orderingArbitrary()), : Base(graph), state_(graph, initialValues), params_(params) {}
gradient_(new VectorValuesOrdered(initialValues.zeroVectors(*ordering_))){}
virtual ~NonlinearConjugateGradientOptimizer() {} virtual ~NonlinearConjugateGradientOptimizer() {}
virtual void iterate(); virtual void iterate();
@ -189,4 +183,3 @@ boost::tuple<V, size_t> nonlinearConjugateGradient(const S &system, const V &ini
} }
#endif

14
tests/testGradientDescentOptimizer.cpp
View File

@ -31,20 +31,20 @@ boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
// 2a. Add Gaussian prior // 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise)); graph += PriorFactor<Pose2>(1, priorMean, priorNoise);
// 2b. Add odometry factors // 2b. Add odometry factors
SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); graph += BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph += BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph += BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph += BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint // 2c. Add pose constraint
SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty)); graph += BetweenFactor<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// 3. Create the data structure to hold the initialEstimate estinmate to the solution // 3. Create the data structure to hold the initialEstimate estimate to the solution
Values initialEstimate; Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2);