Merge branch 'cg-methods' into rosenbrock

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -28,46 +28,6 @@ namespace gtsam {
 
 typedef internal::NonlinearOptimizerState State;
 
-/* ************************************************************************* */
-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;
-}
-
-/* ************************************************************************* */
-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) /
-                        prevGradient.dot(prevGradient));
-  return beta;
-}
-
-/* ************************************************************************* */
-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;
-}
-
-/* ************************************************************************* */
-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) /
-                        -direction.dot(currentGradient - prevGradient));
-  return beta;
-}
-
 /**
  * @brief Return the gradient vector of a nonlinear factor graph
  * @param nfg the graph

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -24,22 +24,48 @@
 namespace gtsam {
 
 /// Fletcher-Reeves formula for computing β, the direction of steepest descent.
-double FletcherReeves(const VectorValues &currentGradient,
+template <typename Gradient>
-                      const VectorValues &prevGradient);
+double FletcherReeves(const Gradient &currentGradient,
+                      const Gradient &prevGradient) {
+  // Fletcher-Reeves: beta = g_n'*g_n/g_n-1'*g_n-1
+  const double beta =
+      currentGradient.dot(currentGradient) / prevGradient.dot(prevGradient);
+  return beta;
+}
 
 /// Polak-Ribiere formula for computing β, the direction of steepest descent.
-double PolakRibiere(const VectorValues &currentGradient,
+template <typename Gradient>
-                    const VectorValues &prevGradient);
+double PolakRibiere(const Gradient &currentGradient,
+                    const Gradient &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) /
+                        prevGradient.dot(prevGradient));
+  return beta;
+}
 
 /// The Hestenes-Stiefel formula for computing β,
 /// the direction of steepest descent.
-double HestenesStiefel(const VectorValues &currentGradient,
+template <typename Gradient>
-                       const VectorValues &prevGradient,
+double HestenesStiefel(const Gradient &currentGradient,
-                       const VectorValues &direction);
+                       const Gradient &prevGradient,
+                       const Gradient &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.
-double DaiYuan(const VectorValues &currentGradient,
+template <typename Gradient>
-               const VectorValues &prevGradient, const VectorValues &direction);
+double DaiYuan(const Gradient &currentGradient, const Gradient &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) /
+                        -direction.dot(currentGradient - prevGradient));
+  return beta;
+}
 
 enum class DirectionMethod {
   FletcherReeves,

gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp

@@ -239,6 +239,49 @@ TEST(NonlinearConjugateGradientOptimizer, Optimization) {
   EXPECT(assert_equal(expected, result, 1e-1));
 }
 
+/* ************************************************************************* */
+/// Test different direction methods
+TEST(NonlinearConjugateGradientOptimizer, DirectionMethods) {
+  const auto [graph, initialEstimate] = generateProblem();
+
+  NonlinearOptimizerParams param;
+  param.maxIterations =
+      500; /* requires a larger number of iterations to converge */
+  param.verbosity = NonlinearOptimizerParams::SILENT;
+
+  // Fletcher-Reeves
+  {
+    NonlinearConjugateGradientOptimizer optimizer(
+        graph, initialEstimate, param, DirectionMethod::FletcherReeves);
+    Values result = optimizer.optimize();
+
+    EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4);
+  }
+  // Polak-Ribiere
+  {
+    NonlinearConjugateGradientOptimizer optimizer(
+        graph, initialEstimate, param, DirectionMethod::PolakRibiere);
+    Values result = optimizer.optimize();
+
+    EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4);
+  }
+  // Hestenes-Stiefel
+  {
+    NonlinearConjugateGradientOptimizer optimizer(
+        graph, initialEstimate, param, DirectionMethod::HestenesStiefel);
+    Values result = optimizer.optimize();
+
+    EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4);
+  }
+  // Dai-Yuan
+  {
+    NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param,
+                                                  DirectionMethod::DaiYuan);
+    Values result = optimizer.optimize();
+
+    EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4);
+  }
+}
 /* ************************************************************************* */
 int main() {
   TestResult tr;