diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 83ca78efb..e5a501b15 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -5,13 +5,10 @@ * @date Jun 11, 2012 */ -#if 0 - #include -#include #include -#include -#include +#include +#include #include @@ -21,18 +18,10 @@ 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 */ -void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, const OrderingOrdered &ordering, VectorValuesOrdered &g) { +VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { // Linearize graph - GaussianFactorGraphOrdered::shared_ptr linear = nfg.linearize(values, ordering); - FactorGraphOrdered jfg; jfg.reserve(linear->size()); - BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, *linear) { - if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) - jfg.push_back((jf)); - else - jfg.push_back(boost::make_shared(*factor)); - } - // compute the gradient direction - gradientAtZero(jfg, g); + GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); + return linear->gradientAtZero(); } 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 { - Gradient result = state.zeroVectors(ordering_); - gradientInPlace(graph_, state, ordering_, result); - return result; + return gradientInPlace(graph_, state); } NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; - scal(alpha, step); - return current.retract(step, ordering_); + step *= alpha; + return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { size_t dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_, *ordering_), state_.values, params_, true /* single iterations */); + boost::tie(state_.values, dummy) = nonlinearConjugateGradient(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(graph_, *ordering_), state_.values, params_, false /* up to convergent */); + boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); state_.error = graph_.error(state_.values); return state_.values; } } /* namespace gtsam */ -#endif diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 5de4c047f..4b12eae6f 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -7,8 +7,6 @@ #pragma once -#if 0 - #include #include #include @@ -28,15 +26,14 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz class System { public: typedef Values State; - typedef VectorValuesOrdered Gradient; + typedef VectorValues Gradient; typedef NonlinearOptimizerParams Parameters; protected: const NonlinearFactorGraph &graph_; - const OrderingOrdered &ordering_; public: - System(const NonlinearFactorGraph &graph, const OrderingOrdered &ordering): graph_(graph), ordering_(ordering) {} + System(const NonlinearFactorGraph &graph): graph_(graph) {} double error(const State &state) const ; Gradient gradient(const State &state) const ; State advance(const State ¤t, const double alpha, const Gradient &g) const ; @@ -51,15 +48,12 @@ public: protected: States state_; Parameters params_; - OrderingOrdered::shared_ptr ordering_; - VectorValuesOrdered::shared_ptr gradient_; public: NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params), ordering_(initialValues.orderingArbitrary()), - gradient_(new VectorValuesOrdered(initialValues.zeroVectors(*ordering_))){} + : Base(graph), state_(graph, initialValues), params_(params) {} virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); @@ -189,4 +183,3 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &ini } -#endif \ No newline at end of file diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 4f556eb0f..cfe81cd2c 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -31,20 +31,20 @@ boost::tuple generateProblem() { // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.add(BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty)); + graph += BetweenFactor(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; Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2);