From 9ee652c04a4b1d7236349d5b0b9763d69c8a3018 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 23 Oct 2023 17:57:07 -0400
Subject: [PATCH] ParameterMatrix as a separate static method

---
 gtsam/basis/Chebyshev.h  | 10 ++++++++++
 gtsam/basis/Chebyshev2.h |  7 ++++++-
 gtsam/basis/FitBasis.h   |  2 +-
 gtsam/basis/Fourier.h    |  5 +++++
 4 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h
index 1c16c47bf..25bd69698 100644
--- a/gtsam/basis/Chebyshev.h
+++ b/gtsam/basis/Chebyshev.h
@@ -34,6 +34,11 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis<Chebyshev1Basis> {
 
   Parameters parameters_;
 
+  /// Return a zero initialized Parameter matrix.
+  static Parameters ParameterMatrix(size_t N) {
+    return Parameters::Zero(N);
+  }
+
   /**
    * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values)
    *
@@ -80,6 +85,11 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis<Chebyshev1Basis> {
 struct GTSAM_EXPORT Chebyshev2Basis : Basis<Chebyshev2Basis> {
   using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
 
+  /// Return a zero initialized Parameter matrix.
+  static Parameters ParameterMatrix(size_t N) {
+    return Parameters::Zero(N);
+  }
+
   /**
    *  Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values).
    *
diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index 55bb39488..6cc18bccc 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -54,7 +54,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   /**
    * @brief Specific Chebyshev point, within [a,b] interval.
    * Default interval is [-1, 1]
-   * 
+   *
    * @param N The degree of the polynomial
    * @param j The index of the Chebyshev point
    * @param a Lower bound of interval (default: -1)
@@ -86,6 +86,11 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
     return points;
   }
 
+  /// Return a zero initialized Parameter matrix.
+  static Parameters ParameterMatrix(size_t N) {
+    return Parameters::Zero(N + 1);
+  }
+
   /**
    * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values)
    * These weights implement barycentric interpolation at a specific x.
diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h
index f5cb99bd7..6e7e809c7 100644
--- a/gtsam/basis/FitBasis.h
+++ b/gtsam/basis/FitBasis.h
@@ -74,7 +74,7 @@ class FitBasis {
       const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
     NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N);
     Values values;
-    values.insert<Parameters>(0, Parameters::Zero(N));
+    values.insert<Parameters>(0, Basis::ParameterMatrix(N));
     GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
     return gfg;
   }
diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h
index eb259bd8a..65d141561 100644
--- a/gtsam/basis/Fourier.h
+++ b/gtsam/basis/Fourier.h
@@ -51,6 +51,11 @@ class FourierBasis : public Basis<FourierBasis> {
     return b;
   }
 
+  /// Return a zero initialized Parameter matrix.
+  static Parameters ParameterMatrix(size_t N) {
+    return Parameters::Zero(N);
+  }
+
   /**
    * @brief Evaluate Real Fourier Weights of size N in interval [a, b],
    *        e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x)