remove System inside NonlinearConjugateGradientOptimizer

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -48,26 +48,27 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
                       new State(initialValues, graph.error(initialValues)))),
       params_(params) {}
 
-double NonlinearConjugateGradientOptimizer::System::error(
+double NonlinearConjugateGradientOptimizer::error(
     const Values& state) const {
   return graph_.error(state);
 }
 
-VectorValues NonlinearConjugateGradientOptimizer::System::gradient(
+VectorValues NonlinearConjugateGradientOptimizer::gradient(
     const Values& state) const {
   return gradientInPlace(graph_, state);
 }
 
-Values NonlinearConjugateGradientOptimizer::System::advance(
+Values NonlinearConjugateGradientOptimizer::advance(
     const Values& current, const double alpha,
     const VectorValues& gradient) const {
   return current.retract(alpha * gradient);
 }
 
 GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
-  const auto [newValues, dummy] = nonlinearConjugateGradient<System, Values>(
+  const auto [newValues, dummy] = nonlinearConjugateGradient<Values>(
-      System(graph_), state_->values, params_, true /* single iteration */);
+      state_->values, params_, true /* single iteration */);
-  state_.reset(new State(newValues, graph_.error(newValues), state_->iterations + 1));
+  state_.reset(
+      new State(newValues, graph_.error(newValues), state_->iterations + 1));
 
   // NOTE(frank): We don't linearize this system, so we must return null here.
   return nullptr;
@@ -75,10 +76,10 @@ GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
 
 const Values& NonlinearConjugateGradientOptimizer::optimize() {
   // Optimize until convergence
-  System system(graph_);
   const auto [newValues, iterations] =
-      nonlinearConjugateGradient(system, state_->values, params_, false);
+      nonlinearConjugateGradient(state_->values, params_, false);
-  state_.reset(new State(std::move(newValues), graph_.error(newValues), iterations));
+  state_.reset(
+      new State(std::move(newValues), graph_.error(newValues), iterations));
   return state_->values;
 }
 

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -24,28 +24,9 @@
 namespace gtsam {
 
 /**  An implementation of the nonlinear CG method using the template below */
-class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
+class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
-
+    : public NonlinearOptimizer {
-  /* a class for the nonlinearConjugateGradient template */
+ public:
-  class System {
-  public:
-    typedef NonlinearOptimizerParams Parameters;
-
-  protected:
-    const NonlinearFactorGraph &graph_;
-
-  public:
-    System(const NonlinearFactorGraph &graph) :
-        graph_(graph) {
-    }
-    double error(const Values &state) const;
-    VectorValues gradient(const Values &state) const;
-    Values advance(const Values &current, const double alpha,
-                  const VectorValues &g) const;
-  };
-
-public:
-
   typedef NonlinearOptimizer Base;
   typedef NonlinearOptimizerParams Parameters;
   typedef std::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
@@ -53,20 +34,23 @@ public:
  protected:
   Parameters params_;
 
-  const NonlinearOptimizerParams& _params() const override {
+  const NonlinearOptimizerParams &_params() const override { return params_; }
-    return params_;
-  }
-
-public:
 
+ public:
   /// Constructor
-  NonlinearConjugateGradientOptimizer(
+  NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph &graph,
-      const NonlinearFactorGraph &graph, const Values &initialValues,
+                                      const Values &initialValues,
-      const Parameters &params = Parameters());
+                                      const Parameters &params = Parameters());
 
   /// Destructor
-  ~NonlinearConjugateGradientOptimizer() override {
+  ~NonlinearConjugateGradientOptimizer() override {}
-  }
+
+  double error(const Values &state) const;
+
+  VectorValues gradient(const Values &state) const;
+
+  Values advance(const Values &current, const double alpha,
+                 const VectorValues &g) const;
 
   /** 
    * Perform a single iteration, returning GaussianFactorGraph corresponding to 
@@ -79,145 +63,147 @@ public:
    * variable assignments.
    */
   const Values& optimize() override;
-};
 
-/** Implement the golden-section line search algorithm */
+  /** Implement the golden-section line search algorithm */
-template<class S, class V, class W>
+  template <class V, class W>
-double lineSearch(const S &system, const V currentValues, const W &gradient) {
+  double lineSearch(const V currentValues, const W &gradient) {
+    /* normalize it such that it becomes a unit vector */
+    const double g = gradient.norm();
 
-  /* normalize it such that it becomes a unit vector */
+    // perform the golden section search algorithm to decide the the optimal
-  const double g = gradient.norm();
+    // 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;
+    double minStep = -1.0 / g, maxStep = 0,
+           newStep = minStep + (maxStep - minStep) / (phi + 1.0);
 
-  // perform the golden section search algorithm to decide the the optimal step size
+    V newValues = advance(currentValues, newStep, gradient);
-  // detail refer to http://en.wikipedia.org/wiki/Golden_section_search
+    double newError = error(newValues);
-  const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau =
-      1e-5;
-  double minStep = -1.0 / g, maxStep = 0, newStep = minStep
-      + (maxStep - minStep) / (phi + 1.0);
 
-  V newValues = system.advance(currentValues, newStep, gradient);
+    while (true) {
-  double newError = system.error(newValues);
+      const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
+      const double testStep = flag ? newStep + resphi * (maxStep - newStep)
+                                   : newStep - resphi * (newStep - minStep);
 
-  while (true) {
+      if ((maxStep - minStep) <
-    const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
+          tau * (std::abs(testStep) + std::abs(newStep))) {
-    const double testStep =
+        return 0.5 * (minStep + maxStep);
-        flag ? newStep + resphi * (maxStep - newStep) :
+      }
-            newStep - resphi * (newStep - minStep);
 
-    if ((maxStep - minStep)
+      const V testValues = advance(currentValues, testStep, gradient);
-        < tau * (std::abs(testStep) + std::abs(newStep))) {
+      const double testError = error(testValues);
-      return 0.5 * (minStep + maxStep);
-    }
 
-    const V testValues = system.advance(currentValues, testStep, gradient);
+      // update the working range
-    const double testError = system.error(testValues);
+      if (testError >= newError) {
-
+        if (flag)
-    // update the working range
+          maxStep = testStep;
-    if (testError >= newError) {
+        else
-      if (flag)
+          minStep = testStep;
-        maxStep = testStep;
-      else
-        minStep = testStep;
-    } else {
-      if (flag) {
-        minStep = newStep;
-        newStep = testStep;
-        newError = testError;
       } else {
-        maxStep = newStep;
+        if (flag) {
-        newStep = testStep;
+          minStep = newStep;
-        newError = testError;
+          newStep = testStep;
+          newError = testError;
+        } else {
+          maxStep = newStep;
+          newStep = testStep;
+          newError = testError;
+        }
       }
     }
-  }
+    return 0.0;
-  return 0.0;
-}
-
-/**
- * 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
- * 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
- */
-template<class S, class V>
-std::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)
-
-  size_t 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 {initial, iteration};
   }
 
-  V currentValues = initial;
+  /**
-  VectorValues currentGradient = system.gradient(currentValues), prevGradient,
+   * Implement the nonlinear conjugate gradient method using the Polak-Ribiere
-               direction = currentGradient;
+   * formula suggested in
+   * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
+   *
+   * The V class
+   * denotes the state or the solution.
+   *
+   * The last parameter is a switch between gradient-descent and conjugate
+   * gradient
+   */
+  template <class V>
+  std::tuple<V, int> nonlinearConjugateGradient(
+      const V &initial, const NonlinearOptimizerParams &params,
+      const bool singleIteration, const bool gradientDescent = false) {
+    // GTSAM_CONCEPT_MANIFOLD_TYPE(V)
 
-  /* do one step of gradient descent */
+    size_t iteration = 0;
-  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
+    // check if we're already close enough
-  if (params.verbosity >= NonlinearOptimizerParams::ERROR)
+    double currentError = error(initial);
-    std::cout << "Initial error: " << currentError << std::endl;
+    if (currentError <= params.errorTol) {
-
+      if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
-  // Iterative loop
+        std::cout << "Exiting, as error = " << currentError << " < "
-  do {
+                  << params.errorTol << std::endl;
-    if (gradientDescent == true) {
+      }
-      direction = system.gradient(currentValues);
+      return {initial, iteration};
-    } else {
-      prevGradient = currentGradient;
-      currentGradient = system.gradient(currentValues);
-      // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1
-      const double beta = std::max(0.0,
-          currentGradient.dot(currentGradient - prevGradient)
-              / prevGradient.dot(prevGradient));
-      direction = currentGradient + (beta * direction);
     }
 
-    alpha = lineSearch(system, currentValues, direction);
+    V currentValues = initial;
+    VectorValues currentGradient = gradient(currentValues), prevGradient,
+                 direction = currentGradient;
 
-    prevValues = currentValues;
+    /* do one step of gradient descent */
-    prevError = currentError;
+    V prevValues = currentValues;
-
+    double prevError = currentError;
-    currentValues = system.advance(prevValues, alpha, direction);
+    double alpha = lineSearch(currentValues, direction);
-    currentError = system.error(currentValues);
+    currentValues = advance(prevValues, alpha, direction);
-
+    currentError = error(currentValues);
-    // User hook:
-    if (params.iterationHook)
-      params.iterationHook(iteration, prevError, currentError);
 
     // Maybe show output
     if (params.verbosity >= NonlinearOptimizerParams::ERROR)
-      std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;
+      std::cout << "Initial error: " << currentError << std::endl;
-  } while (++iteration < params.maxIterations && !singleIteration
-      && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
-          params.errorTol, prevError, currentError, params.verbosity));
 
-  // Printing if verbose
+    // Iterative loop
-  if (params.verbosity >= NonlinearOptimizerParams::ERROR
+    do {
-      && iteration >= params.maxIterations)
+      if (gradientDescent == true) {
-    std::cout
+        direction = gradient(currentValues);
-        << "nonlinearConjugateGradient: Terminating because reached maximum iterations"
+      } else {
-        << std::endl;
+        prevGradient = currentGradient;
+        currentGradient = gradient(currentValues);
+        // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1
+        const double beta =
+            std::max(0.0, currentGradient.dot(currentGradient - prevGradient) /
+                              prevGradient.dot(prevGradient));
+        direction = currentGradient + (beta * direction);
+      }
 
-  return {currentValues, iteration};
+      alpha = lineSearch(currentValues, direction);
-}
+
+      prevValues = currentValues;
+      prevError = currentError;
+
+      currentValues = advance(prevValues, alpha, direction);
+      currentError = error(currentValues);
+
+      // User hook:
+      if (params.iterationHook)
+        params.iterationHook(iteration, prevError, currentError);
+
+      // Maybe show output
+      if (params.verbosity >= NonlinearOptimizerParams::ERROR)
+        std::cout << "iteration: " << iteration
+                  << ", currentError: " << currentError << std::endl;
+    } while (++iteration < params.maxIterations && !singleIteration &&
+             !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
+                               params.errorTol, prevError, currentError,
+                               params.verbosity));
+
+    // Printing if verbose
+    if (params.verbosity >= NonlinearOptimizerParams::ERROR &&
+        iteration >= params.maxIterations)
+      std::cout << "nonlinearConjugateGradient: Terminating because reached "
+                   "maximum iterations"
+                << std::endl;
+
+    return {currentValues, iteration};
+  }
+};
 
 } // \ namespace gtsam