From 2d4ee5057a9651bd3e9a3e392570ccb0d5d807ab Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 18 Oct 2024 10:30:01 -0400
Subject: [PATCH] template the directional methods

---
 .../NonlinearConjugateGradientOptimizer.cpp   | 40 -----------------
 .../NonlinearConjugateGradientOptimizer.h     | 44 +++++++++++++++----
 2 files changed, 35 insertions(+), 49 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
index 438eac7a8..8b8542779 100644
--- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
+++ b/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
diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
index 09b932d42..1aee33a72 100644
--- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
+++ b/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,
-                      const VectorValues &prevGradient);
+template <typename Gradient>
+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,
-                    const VectorValues &prevGradient);
+template <typename Gradient>
+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,
-                       const VectorValues &prevGradient,
-                       const VectorValues &direction);
+template <typename Gradient>
+double HestenesStiefel(const Gradient &currentGradient,
+                       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,
-               const VectorValues &prevGradient, const VectorValues &direction);
+template <typename Gradient>
+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,