nonlinearConjugateGradient accepts direction method

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -28,18 +28,18 @@ namespace gtsam {
 
 typedef internal::NonlinearOptimizerState State;
 
-/// Fletcher-Reeves formula for computing β, the direction of steepest descent.
-static double FletcherReeves(const VectorValues& currentGradient,
-                             const VectorValues& prevGradient) {
+/* ************************************************************************* */
+double FletcherReeves(const VectorValues& currentGradient,
+                      const VectorValues& prevGradient) {
   // Fletcher-Reeves: beta = g_n'*g_n/g_n-1'*g_n-1
   const double beta = std::max(0.0, currentGradient.dot(currentGradient) /
                                         prevGradient.dot(prevGradient));
   return beta;
 }
 
-/// Polak-Ribiere formula for computing β, the direction of steepest descent.
-static double PolakRibiere(const VectorValues& currentGradient,
-                           const VectorValues& prevGradient) {
+/* ************************************************************************* */
+double PolakRibiere(const VectorValues& currentGradient,
+                    const VectorValues& prevGradient) {
   // Polak-Ribiere: beta = g_n'*(g_n-g_n-1)/g_n-1'*g_n-1
   const double beta =
       std::max(0.0, currentGradient.dot(currentGradient - prevGradient) /
@@ -47,20 +47,20 @@ static double PolakRibiere(const VectorValues& currentGradient,
   return beta;
 }
 
-/// The Hestenes-Stiefel formula for computing β, the direction of steepest descent.
-static double HestenesStiefel(const VectorValues& currentGradient,
-                              const VectorValues& prevGradient,
-                              const VectorValues& direction) {
+/* ************************************************************************* */
+double HestenesStiefel(const VectorValues& currentGradient,
+                       const VectorValues& prevGradient,
+                       const VectorValues& direction) {
   // Hestenes-Stiefel: beta = g_n'*(g_n-g_n-1)/(-s_n-1')*(g_n-g_n-1)
   VectorValues d = currentGradient - prevGradient;
   const double beta = std::max(0.0, currentGradient.dot(d) / -direction.dot(d));
   return beta;
 }
 
-/// The Dai-Yuan formula for computing β, the direction of steepest descent.
-static double DaiYuan(const VectorValues& currentGradient,
-                      const VectorValues& prevGradient,
-                      const VectorValues& direction) {
+/* ************************************************************************* */
+double DaiYuan(const VectorValues& currentGradient,
+               const VectorValues& prevGradient,
+               const VectorValues& direction) {
   // Dai-Yuan: beta = g_n'*g_n/(-s_n-1')*(g_n-g_n-1)
   const double beta =
       std::max(0.0, currentGradient.dot(currentGradient) /
@@ -110,7 +110,8 @@ NonlinearConjugateGradientOptimizer::System::advance(const State& current,
 
 GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
   const auto [newValues, dummy] = nonlinearConjugateGradient<System, Values>(
-      System(graph_), state_->values, params_, true /* single iteration */);
+      System(graph_), state_->values, params_, true /* single iteration */,
+      directionMethod_);
   state_.reset(
       new State(newValues, graph_.error(newValues), state_->iterations + 1));
 
@@ -121,8 +122,8 @@ 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);
+  const auto [newValues, iterations] = nonlinearConjugateGradient(
+      system, state_->values, params_, false, directionMethod_);
   state_.reset(
       new State(std::move(newValues), graph_.error(newValues), iterations));
   return state_->values;

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -23,6 +23,31 @@
 
 namespace gtsam {
 
+/// Fletcher-Reeves formula for computing β, the direction of steepest descent.
+double FletcherReeves(const VectorValues &currentGradient,
+                      const VectorValues &prevGradient);
+
+/// Polak-Ribiere formula for computing β, the direction of steepest descent.
+double PolakRibiere(const VectorValues &currentGradient,
+                    const VectorValues &prevGradient);
+
+/// The Hestenes-Stiefel formula for computing β,
+/// the direction of steepest descent.
+double HestenesStiefel(const VectorValues &currentGradient,
+                       const VectorValues &prevGradient,
+                       const VectorValues &direction);
+
+/// The Dai-Yuan formula for computing β, the direction of steepest descent.
+double DaiYuan(const VectorValues &currentGradient,
+               const VectorValues &prevGradient, const VectorValues &direction);
+
+enum class DirectionMethod {
+  FletcherReeves,
+  PolakRibiere,
+  HestenesStiefel,
+  DaiYuan
+};
+
 /**  An implementation of the nonlinear CG method using the template below */
 class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
     : public NonlinearOptimizer {
@@ -49,13 +74,6 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
   typedef NonlinearOptimizerParams Parameters;
   typedef std::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
 
-  enum class DirectionMethod {
-    FletcherReeves,
-    PolakRibiere,
-    HestenesStiefel,
-    DaiYuan
-  };
-
  protected:
   Parameters params_;
   DirectionMethod directionMethod_ = DirectionMethod::PolakRibiere;
@@ -149,7 +167,9 @@ double lineSearch(const S &system, const V currentValues, const W &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) {
+    const bool singleIteration,
+    const DirectionMethod &directionMethod = DirectionMethod::PolakRibiere,
+    const bool gradientDescent = false) {
   // GTSAM_CONCEPT_MANIFOLD_TYPE(V)
 
   size_t iteration = 0;
@@ -186,10 +206,23 @@ std::tuple<V, int> nonlinearConjugateGradient(
     } 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));
+
+      double beta;
+      switch (directionMethod) {
+        case DirectionMethod::FletcherReeves:
+          beta = FletcherReeves(currentGradient, prevGradient);
+          break;
+        case DirectionMethod::PolakRibiere:
+          beta = PolakRibiere(currentGradient, prevGradient);
+          break;
+        case DirectionMethod::HestenesStiefel:
+          beta = HestenesStiefel(currentGradient, prevGradient, direction);
+          break;
+        case DirectionMethod::DaiYuan:
+          beta = DaiYuan(currentGradient, prevGradient, direction);
+          break;
+      }
+
       direction = currentGradient + (beta * direction);
     }