Cleaned up (obsolete?) CG-based solver by Yong-Dian

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -1,7 +1,18 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
- * @file   GradientDescentOptimizer.cpp
+ * @file   NonlinearConjugateGradientOptimizer.cpp
- * @brief  
+ * @brief  Simple non-linear optimizer that solves using *non-preconditioned* CG
- * @author ydjian
+ * @author Yong-Dian Jian
  * @date   Jun 11, 2012
  */
 
@@ -16,36 +27,49 @@ 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 */
+ * @brief Return the gradient vector of a nonlinear factor graph
-VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) {
+ * @param nfg the graph
+ * @param values a linearization point
+ * Can be moved to NonlinearFactorGraph.h if desired
+ */
+static 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 {
+double NonlinearConjugateGradientOptimizer::System::error(
+    const State &state) const {
   return graph_.error(state);
 }
 
-NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const {
+NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(
+    const State &state) const {
   return gradientInPlace(graph_, state);
 }
-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;
   step *= alpha;
   return current.retract(step);
 }
 
 void NonlinearConjugateGradientOptimizer::iterate() {
-  int dummy ;
+  int dummy;
-  boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>(System(graph_), 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_.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 */);
+  // Optimize until convergence
+  System system(graph_);
+  boost::tie(state_.values, state_.iterations) = //
+      nonlinearConjugateGradient(system, state_.values, params_, false);
   state_.error = graph_.error(state_.values);
   return state_.values;
 }

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -1,8 +1,19 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
- * @file   GradientDescentOptimizer.cpp
+ * @file   NonlinearConjugateGradientOptimizer.h
- * @brief
+ * @brief  Simple non-linear optimizer that solves using *non-preconditioned* CG
  * @author Yong-Dian Jian
- * @date   Jun 11, 2012
+ * @date   June 11, 2012
  */
 
 #pragma once
@@ -13,15 +24,18 @@
 
 namespace gtsam {
 
-/**  An implementation of the nonlinear cg method using the template below */
+/**  An implementation of the nonlinear CG method using the template below */
-class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState {
+class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState {
 public:
   typedef NonlinearOptimizerState Base;
-  NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values)
+  NonlinearConjugateGradientState(const NonlinearFactorGraph& graph,
-    : Base(graph, values) {}
+      const Values& values) :
+      Base(graph, values) {
+  }
 };
 
-class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
+class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer {
+
   /* a class for the nonlinearConjugateGradient template */
   class System {
   public:
@@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz
     const NonlinearFactorGraph &graph_;
 
   public:
-    System(const NonlinearFactorGraph &graph): graph_(graph) {}
+    System(const NonlinearFactorGraph &graph) :
-    double error(const State &state) const ;
+        graph_(graph) {
-    Gradient gradient(const State &state) const ;
+    }
-    State advance(const State &current, const double alpha, const Gradient &g) const ;
+    double error(const State &state) const;
+    Gradient gradient(const State &state) const;
+    State advance(const State &current, const double alpha,
+        const Gradient &g) const;
   };
 
 public:
+
   typedef NonlinearOptimizer Base;
-  typedef NonlinearConjugateGradientState States;
   typedef NonlinearOptimizerParams Parameters;
   typedef boost::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
 
 protected:
-  States state_;
+  NonlinearConjugateGradientState state_;
   Parameters params_;
 
 public:
 
-  NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
+  /// Constructor
-                                      const Parameters& params = Parameters())
+  NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph,
-    : Base(graph), state_(graph, initialValues), params_(params) {}
+      const Values& initialValues, const Parameters& params = Parameters()) :
+      Base(graph), state_(graph, initialValues), params_(params) {
+  }
+
+  /// Destructor
+  virtual ~NonlinearConjugateGradientOptimizer() {
+  }
 
-  virtual ~NonlinearConjugateGradientOptimizer() {}
   virtual void iterate();
-  virtual const Values& optimize ();
+  virtual const Values& optimize();
-  virtual const NonlinearOptimizerState& _state() const { return state_; }
+  virtual const NonlinearOptimizerState& _state() const {
-  virtual const NonlinearOptimizerParams& _params() const { return params_; }
+    return state_;
+  }
+  virtual const NonlinearOptimizerParams& _params() const {
+    return params_;
+  }
 };
 
 /** Implement the golden-section line search algorithm */
-template <class S, class V, class W>
+template<class S, class V, class W>
 double lineSearch(const S &system, const V currentValues, const W &gradient) {
 
   /* normalize it such that it becomes a unit vector */
@@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
 
   // perform the golden section search algorithm to decide the the optimal step size
   // detail refer to http://en.wikipedia.org/wiki/Golden_section_search
-  const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5;
+  const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau =
-  double minStep = -1.0/g, maxStep = 0,
+      1e-5;
-         newStep = minStep + (maxStep-minStep) / (phi+1.0) ;
+  double minStep = -1.0 / g, maxStep = 0, newStep = minStep
+      + (maxStep - minStep) / (phi + 1.0);
 
   V newValues = system.advance(currentValues, newStep, gradient);
   double newError = system.error(newValues);
 
   while (true) {
-    const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ;
+    const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
-    const double testStep = flag ?
+    const double testStep =
-                            newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep);
+        flag ? newStep + resphi * (maxStep - newStep) :
+            newStep - resphi * (newStep - minStep);
 
-    if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) {
+    if ((maxStep - minStep)
-      return  0.5*(minStep+maxStep);
+        < tau * (std::fabs(testStep) + std::fabs(newStep))) {
+      return 0.5 * (minStep + maxStep);
     }
 
     const V testValues = system.advance(currentValues, testStep, gradient);
     const double testError = system.error(testValues);
 
     // update the working range
-    if ( testError >= newError ) {
+    if (testError >= newError) {
-      if ( flag ) maxStep = testStep;
+      if (flag)
-      else minStep = testStep;
+        maxStep = testStep;
-    }
+      else
-    else {
+        minStep = testStep;
-      if ( flag ) {
+    } else {
+      if (flag) {
         minStep = newStep;
         newStep = testStep;
         newError = testError;
-      }
+      } else {
-      else {
         maxStep = newStep;
         newStep = testStep;
         newError = testError;
@@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
 }
 
 /**
- * Implement the nonlinear conjugate gradient method using the Polak-Ribieve formula suggested in
+ * Implement the nonlinear conjugate gradient method using the Polak-Ribiere 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
@@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
  *
  * The last parameter is a switch between gradient-descent and conjugate gradient
  */
-template <class S, class V>
+template<class S, class V>
-boost::tuple<V, int> nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const bool gradientDescent = false) {
+boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
+    const V &initial, const NonlinearOptimizerParams &params,
+    const bool singleIteration, const bool gradientDescent = false) {
 
   // GTSAM_CONCEPT_MANIFOLD_TYPE(V);
 
@@ -129,57 +160,67 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system, const V &initia
 
   // check if we're already close enough
   double currentError = system.error(initial);
-  if(currentError <= params.errorTol) {
+  if (currentError <= params.errorTol) {
-    if (params.verbosity >= NonlinearOptimizerParams::ERROR){
+    if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
-      std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl;
+      std::cout << "Exiting, as error = " << currentError << " < "
+          << params.errorTol << std::endl;
     }
     return boost::tie(initial, iteration);
   }
 
   V currentValues = initial;
-  typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient,
+  typename S::Gradient currentGradient = system.gradient(currentValues),
-                       direction = currentGradient;
+      prevGradient, direction = currentGradient;
 
   /* do one step of gradient descent */
-  V prevValues = currentValues; double prevError = currentError;
+  V prevValues = currentValues;
+  double prevError = currentError;
   double alpha = lineSearch(system, currentValues, direction);
   currentValues = system.advance(prevValues, alpha, direction);
   currentError = system.error(currentValues);
 
   // Maybe show output
-  if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl;
+  if (params.verbosity >= NonlinearOptimizerParams::ERROR)
+    std::cout << "Initial error: " << currentError << std::endl;
 
   // Iterative loop
   do {
-    if ( gradientDescent == true) {
+    if (gradientDescent == true) {
       direction = system.gradient(currentValues);
-    }
+    } else {
-    else {
       prevGradient = currentGradient;
       currentGradient = system.gradient(currentValues);
-      const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient));
+      // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1
-      direction = currentGradient + (beta*direction);
+      const double beta = std::max(0.0,
+          currentGradient.dot(currentGradient - prevGradient)
+              / currentGradient.dot(currentGradient));
+      direction = currentGradient + (beta * direction);
     }
 
     alpha = lineSearch(system, currentValues, direction);
 
-    prevValues = currentValues; prevError = currentError;
+    prevValues = currentValues;
+    prevError = currentError;
 
     currentValues = system.advance(prevValues, alpha, direction);
     currentError = system.error(currentValues);
 
     // Maybe show output
-    if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl;
+    if (params.verbosity >= NonlinearOptimizerParams::ERROR)
-  } while( ++iteration < params.maxIterations &&
+      std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;
-           !singleIteration &&
+  } while (++iteration < params.maxIterations && !singleIteration
-           !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity));
+      && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
+          params.errorTol, prevError, currentError, params.verbosity));
 
   // Printing if verbose
-  if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations)
+  if (params.verbosity >= NonlinearOptimizerParams::ERROR
-    std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl;
+      && iteration >= params.maxIterations)
+    std::cout
+        << "nonlinearConjugateGradient: Terminating because reached maximum iterations"
+        << std::endl;
 
   return boost::tie(currentValues, iteration);
 }
 
-}
+} // \ namespace gtsam
 

tests/testGradientDescentOptimizer.cpp

@@ -1,7 +1,14 @@
+/**
+ * @file   NonlinearConjugateGradientOptimizer.cpp
+ * @brief  Test simple CG optimizer
+ * @author Yong-Dian Jian
+ * @date   June 11, 2012
+ */
+
 /**
  * @file   testGradientDescentOptimizer.cpp
- * @brief  
+ * @brief  Small test of NonlinearConjugateGradientOptimizer
- * @author ydjian
+ * @author Yong-Dian Jian
  * @date   Jun 11, 2012
  */
 
@@ -22,7 +29,7 @@
 using namespace std;
 using namespace gtsam;
 
-
+// Generate a small PoseSLAM problem
 boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
 
   // 1. Create graph container and add factors to it
@@ -56,49 +63,23 @@ boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
 }
 
 /* ************************************************************************* */
-TEST(optimize, GradientDescentOptimizer) {
+TEST(NonlinearConjugateGradientOptimizer, Optimize) {
 
   NonlinearFactorGraph graph;
   Values initialEstimate;
 
   boost::tie(graph, initialEstimate) = generateProblem();
-  // cout << "initial error = " << graph.error(initialEstimate) << endl ;
+    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;
+  param.verbosity = NonlinearOptimizerParams::ERROR;
 
   NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
   Values result = optimizer.optimize();
-//  cout << "gd1 solver final error = " << graph.error(result) << endl;
+    cout << "cg final error = " << graph.error(result) << endl;
 
-  /* the optimality of the solution is not comparable to the */
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4);
-  DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
-
-  CHECK(1);
-}
-
-/* ************************************************************************* */
-TEST(optimize, ConjugateGradientOptimizer) {
-
-  NonlinearFactorGraph graph;
-  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;
-
-  NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
-  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);
 }
 
 /* ************************************************************************* */