move all implementation to cpp file

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -16,11 +16,11 @@
  * @date   Jun 11, 2012
  */
 
-#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
-#include <gtsam/nonlinear/internal/NonlinearOptimizerState.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/internal/NonlinearOptimizerState.h>
 
 #include <cmath>
 
@@ -34,8 +34,8 @@ typedef internal::NonlinearOptimizerState State;
  * @param values a linearization point
  * Can be moved to NonlinearFactorGraph.h if desired
  */
-static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg,
+static VectorValues gradientInPlace(const NonlinearFactorGraph& nfg,
-    const Values &values) {
+                                    const Values& values) {
   // Linearize graph
   GaussianFactorGraph::shared_ptr linear = nfg.linearize(values);
   return linear->gradientAtZero();
@@ -48,8 +48,7 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
                       new State(initialValues, graph.error(initialValues)))),
       params_(params) {}
 
-double NonlinearConjugateGradientOptimizer::error(
+double NonlinearConjugateGradientOptimizer::error(const Values& state) const {
-    const Values& state) const {
   return graph_.error(state);
 }
 
@@ -83,5 +82,129 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() {
   return state_->values;
 }
 
-} /* namespace gtsam */
+double NonlinearConjugateGradientOptimizer::lineSearch(
+    const Values& currentValues, const VectorValues& gradient) const {
+  /* normalize it such that it becomes a unit vector */
+  const double g = gradient.norm();
 
+  // 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;
+  double minStep = -1.0 / g, maxStep = 0,
+         newStep = minStep + (maxStep - minStep) / (phi + 1.0);
+
+  Values newValues = advance(currentValues, newStep, gradient);
+  double newError = error(newValues);
+
+  while (true) {
+    const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
+    const double testStep = flag ? newStep + resphi * (maxStep - newStep)
+                                 : newStep - resphi * (newStep - minStep);
+
+    if ((maxStep - minStep) < tau * (std::abs(testStep) + std::abs(newStep))) {
+      return 0.5 * (minStep + maxStep);
+    }
+
+    const Values testValues = advance(currentValues, testStep, gradient);
+    const double testError = error(testValues);
+
+    // update the working range
+    if (testError >= newError) {
+      if (flag)
+        maxStep = testStep;
+      else
+        minStep = testStep;
+    } else {
+      if (flag) {
+        minStep = newStep;
+        newStep = testStep;
+        newError = testError;
+      } else {
+        maxStep = newStep;
+        newStep = testStep;
+        newError = testError;
+      }
+    }
+  }
+  return 0.0;
+}
+
+std::tuple<Values, int>
+NonlinearConjugateGradientOptimizer::nonlinearConjugateGradient(
+    const Values& initial, const NonlinearOptimizerParams& params,
+    const bool singleIteration, const bool gradientDescent) const {
+  size_t iteration = 0;
+
+  // check if we're already close enough
+  double currentError = error(initial);
+  if (currentError <= params.errorTol) {
+    if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
+      std::cout << "Exiting, as error = " << currentError << " < "
+                << params.errorTol << std::endl;
+    }
+    return {initial, iteration};
+  }
+
+  Values currentValues = initial;
+  VectorValues currentGradient = gradient(currentValues), prevGradient,
+               direction = currentGradient;
+
+  /* do one step of gradient descent */
+  Values prevValues = currentValues;
+  double prevError = currentError;
+  double alpha = lineSearch(currentValues, direction);
+  currentValues = advance(prevValues, alpha, direction);
+  currentError = error(currentValues);
+
+  // Maybe show output
+  if (params.verbosity >= NonlinearOptimizerParams::ERROR)
+    std::cout << "Initial error: " << currentError << std::endl;
+
+  // Iterative loop
+  do {
+    if (gradientDescent == true) {
+      direction = gradient(currentValues);
+    } else {
+      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);
+    }
+
+    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 */

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -65,54 +65,8 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
   const Values &optimize() override;
 
   /** Implement the golden-section line search algorithm */
-  double lineSearch(const Values &currentValues, const VectorValues &gradient) const {
+  double lineSearch(const Values &currentValues,
-    /* normalize it such that it becomes a unit vector */
+                    const VectorValues &gradient) const;
-    const double g = gradient.norm();
-
-    // 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;
-    double minStep = -1.0 / g, maxStep = 0,
-           newStep = minStep + (maxStep - minStep) / (phi + 1.0);
-
-    Values newValues = advance(currentValues, newStep, gradient);
-    double newError = error(newValues);
-
-    while (true) {
-      const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
-      const double testStep = flag ? newStep + resphi * (maxStep - newStep)
-                                   : newStep - resphi * (newStep - minStep);
-
-      if ((maxStep - minStep) <
-          tau * (std::abs(testStep) + std::abs(newStep))) {
-        return 0.5 * (minStep + maxStep);
-      }
-
-      const Values testValues = advance(currentValues, testStep, gradient);
-      const double testError = error(testValues);
-
-      // update the working range
-      if (testError >= newError) {
-        if (flag)
-          maxStep = testStep;
-        else
-          minStep = testStep;
-      } else {
-        if (flag) {
-          minStep = newStep;
-          newStep = testStep;
-          newError = testError;
-        } else {
-          maxStep = newStep;
-          newStep = testStep;
-          newError = testError;
-        }
-      }
-    }
-    return 0.0;
-  }
 
   /**
    * Implement the nonlinear conjugate gradient method using the Polak-Ribiere
@@ -124,78 +78,7 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
    */
   std::tuple<Values, int> nonlinearConjugateGradient(
       const Values &initial, const NonlinearOptimizerParams &params,
-      const bool singleIteration, const bool gradientDescent = false) const {
+      const bool singleIteration, const bool gradientDescent = false) const;
-    size_t iteration = 0;
-
-    // check if we're already close enough
-    double currentError = error(initial);
-    if (currentError <= params.errorTol) {
-      if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
-        std::cout << "Exiting, as error = " << currentError << " < "
-                  << params.errorTol << std::endl;
-      }
-      return {initial, iteration};
-    }
-
-    Values currentValues = initial;
-    VectorValues currentGradient = gradient(currentValues), prevGradient,
-                 direction = currentGradient;
-
-    /* do one step of gradient descent */
-    Values prevValues = currentValues;
-    double prevError = currentError;
-    double alpha = lineSearch(currentValues, direction);
-    currentValues = advance(prevValues, alpha, direction);
-    currentError = error(currentValues);
-
-    // Maybe show output
-    if (params.verbosity >= NonlinearOptimizerParams::ERROR)
-      std::cout << "Initial error: " << currentError << std::endl;
-
-    // Iterative loop
-    do {
-      if (gradientDescent == true) {
-        direction = gradient(currentValues);
-      } else {
-        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);
-      }
-
-      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