add a generic nonlinear cg solver

gtsam/linear/VectorValues.cpp

@@ -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));

gtsam/linear/VectorValues.h

@@ -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();

gtsam/nonlinear/GradientDescentOptimizer.cpp

@@ -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() {
+Values ConjugateGradientOptimizer::optimize() {
-  return gradientDescent<System, Values>(System(graph_, *ordering_), initialEstimate_, params_);
+  return conjugateGradient<System, Values>(System(graph_, *ordering_), initialEstimate_, params_, !cg_);
 }
 
 } /* namespace gtsam */

gtsam/nonlinear/GradientDescentOptimizer.h

@@ -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 ConjugateGradientOptimizer {
-class GradientDescentParams2 : public NonlinearOptimizerParams {
-public:
-  GradientDescentParams2(){}
-};
-
-class GradientDescentOptimizer2 {
 
   class System {
 
@@ -102,8 +85,8 @@ class GradientDescentOptimizer2 {
 
 public:
 
-  typedef GradientDescentParams2 Parameters;
+  typedef NonlinearOptimizerParams Parameters;
-  typedef boost::shared_ptr<GradientDescentOptimizer2> shared_ptr;
+  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;
+template <class S, class V>
-  double currentError = system.error(currentValues), prevError;
+V conjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool gradientDescent) {
-  Index iteration = 0;
 
   // 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;
+    if ( gradientDescent == true) {
-    prevError = currentError;
+      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;
 }
 
+
+
+
+
+
 }

tests/testGradientDescentOptimizer.cpp

@@ -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); }
 /* ************************************************************************* */