remove templating and make all Basis code dynamic

gtsam/basis/Basis.cpp

@@ -0,0 +1,33 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Basis.cpp
+ * @brief Compute an interpolating basis
+ * @author Varun Agrawal
+ * @date June 20, 2023
+ */
+
+#include <gtsam/basis/Basis.h>
+
+namespace gtsam {
+
+Matrix kroneckerProductIdentity(size_t M, const Weights& w) {
+  Matrix result(M, w.cols() * M);
+  result.setZero();
+
+  for (int i = 0; i < w.cols(); i++) {
+    result.block(0, i * M, M, M).diagonal().array() = w(i);
+  }
+  return result;
+}
+
+}  // namespace gtsam

gtsam/basis/Basis.h

@@ -81,16 +81,7 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
  *
  * @ingroup basis
  */
-template <size_t M>
+Matrix kroneckerProductIdentity(size_t M, const Weights& w);
-Matrix kroneckerProductIdentity(const Weights& w) {
-  Matrix result(M, w.cols() * M);
-  result.setZero();
-
-  for (int i = 0; i < w.cols(); i++) {
-    result.block(0, i * M, M, M).diagonal().array() = w(i);
-  }
-  return result;
-}
 
 /**
  * CRTP Base class for function bases
@@ -174,13 +165,13 @@ class Basis {
    * value x. When given a specific M*N parameters, returns an M-vector the M
    * corresponding functions at x, possibly with Jacobians wrpt the parameters.
    */
-  template <int M>
   class VectorEvaluationFunctor : protected EvaluationFunctor {
    protected:
-    using VectorM = Eigen::Matrix<double, M, 1>;
+    using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
-    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
     Jacobian H_;
 
+    size_t M_;
+
     /**
      * Calculate the `M*(M*N)` Jacobian of this functor with respect to
      * the M*N parameter matrix `P`.
@@ -190,7 +181,7 @@ class Basis {
      * i.e., the Kronecker product of weights_ with the MxM identity matrix.
      */
     void calculateJacobian() {
-      H_ = kroneckerProductIdentity<M>(this->weights_);
+      H_ = kroneckerProductIdentity(M_, this->weights_);
     }
 
    public:
@@ -200,25 +191,26 @@ class Basis {
     VectorEvaluationFunctor() {}
 
     /// Default Constructor
-    VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
+    VectorEvaluationFunctor(size_t M, size_t N, double x)
+        : EvaluationFunctor(N, x), M_(M) {
       calculateJacobian();
     }
 
     /// Constructor, with interval [a,b]
-    VectorEvaluationFunctor(size_t N, double x, double a, double b)
+    VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b)
-        : EvaluationFunctor(N, x, a, b) {
+        : EvaluationFunctor(N, x, a, b), M_(M) {
       calculateJacobian();
     }
 
     /// M-dimensional evaluation
-    VectorM apply(const ParameterMatrix& P,
+    Vector apply(const ParameterMatrix& P,
                  OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
       if (H) *H = H_;
       return P.matrix() * this->weights_.transpose();
     }
 
     /// c++ sugar
-    VectorM operator()(const ParameterMatrix& P,
+    Vector operator()(const ParameterMatrix& P,
                       OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
       return apply(P, H);
     }
@@ -231,13 +223,14 @@ class Basis {
    *
    * This component is specified by the row index i, with 0<i<M.
    */
-  template <int M>
   class VectorComponentFunctor : public EvaluationFunctor {
    protected:
     using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
-    size_t rowIndex_;
     Jacobian H_;
 
+    size_t M_;
+    size_t rowIndex_;
+
     /*
      * Calculate the `1*(M*N)` Jacobian of this functor with respect to
      * the M*N parameter matrix `P`.
@@ -248,9 +241,9 @@ class Basis {
      * MxM identity matrix. See also VectorEvaluationFunctor.
      */
     void calculateJacobian(size_t N) {
-      H_.setZero(1, M * N);
+      H_.setZero(1, M_ * N);
       for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
-        H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
+        H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j);
     }
 
    public:
@@ -258,14 +251,15 @@ class Basis {
     VectorComponentFunctor() {}
 
     /// Construct with row index
-    VectorComponentFunctor(size_t N, size_t i, double x)
+    VectorComponentFunctor(size_t M, size_t N, size_t i, double x)
-        : EvaluationFunctor(N, x), rowIndex_(i) {
+        : EvaluationFunctor(N, x), M_(M), rowIndex_(i) {
       calculateJacobian(N);
     }
 
     /// Construct with row index and interval
-    VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
+    VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a,
-        : EvaluationFunctor(N, x, a, b), rowIndex_(i) {
+                           double b)
+        : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) {
       calculateJacobian(N);
     }
 
@@ -297,21 +291,20 @@ class Basis {
    * 3D rotation.
    */
   template <class T>
-  class ManifoldEvaluationFunctor
+  class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
-      : public VectorEvaluationFunctor<traits<T>::dimension> {
     enum { M = traits<T>::dimension };
-    using Base = VectorEvaluationFunctor<M>;
+    using Base = VectorEvaluationFunctor;
 
    public:
     /// For serialization
     ManifoldEvaluationFunctor() {}
 
     /// Default Constructor
-    ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}
+    ManifoldEvaluationFunctor(size_t N, double x) : Base(M, N, x) {}
 
     /// Constructor, with interval [a,b]
     ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
-        : Base(N, x, a, b) {}
+        : Base(M, N, x, a, b) {}
 
     /// Manifold evaluation
     T apply(const ParameterMatrix& P,
@@ -396,13 +389,13 @@ class Basis {
    * returns an M-vector the M corresponding function derivatives at x, possibly
    * with Jacobians wrpt the parameters.
    */
-  template <int M>
   class VectorDerivativeFunctor : protected DerivativeFunctorBase {
    protected:
-    using VectorM = Eigen::Matrix<double, M, 1>;
+    using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
-    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
     Jacobian H_;
 
+    size_t M_;
+
     /**
      * Calculate the `M*(M*N)` Jacobian of this functor with respect to
      * the M*N parameter matrix `P`.
@@ -412,7 +405,7 @@ class Basis {
      * i.e., the Kronecker product of weights_ with the MxM identity matrix.
      */
     void calculateJacobian() {
-      H_ = kroneckerProductIdentity<M>(this->weights_);
+      H_ = kroneckerProductIdentity(M_, this->weights_);
     }
 
    public:
@@ -422,23 +415,24 @@ class Basis {
     VectorDerivativeFunctor() {}
 
     /// Default Constructor
-    VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
+    VectorDerivativeFunctor(size_t M, size_t N, double x)
+        : DerivativeFunctorBase(N, x), M_(M) {
       calculateJacobian();
     }
 
     /// Constructor, with optional interval [a,b]
-    VectorDerivativeFunctor(size_t N, double x, double a, double b)
+    VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b)
-        : DerivativeFunctorBase(N, x, a, b) {
+        : DerivativeFunctorBase(N, x, a, b), M_(M) {
       calculateJacobian();
     }
 
-    VectorM apply(const ParameterMatrix& P,
+    Vector apply(const ParameterMatrix& P,
                  OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
       if (H) *H = H_;
       return P.matrix() * this->weights_.transpose();
     }
     /// c++ sugar
-    VectorM operator()(const ParameterMatrix& P,
+    Vector operator()(const ParameterMatrix& P,
                       OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
       return apply(P, H);
     }
@@ -451,13 +445,14 @@ class Basis {
    *
    * This component is specified by the row index i, with 0<i<M.
    */
-  template <int M>
   class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
    protected:
     using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
-    size_t rowIndex_;
     Jacobian H_;
 
+    size_t M_;
+    size_t rowIndex_;
+
     /*
      * Calculate the `1*(M*N)` Jacobian of this functor with respect to
      * the M*N parameter matrix `P`.
@@ -468,9 +463,9 @@ class Basis {
      * MxM identity matrix. See also VectorDerivativeFunctor.
      */
     void calculateJacobian(size_t N) {
-      H_.setZero(1, M * N);
+      H_.setZero(1, M_ * N);
       for (int j = 0; j < this->weights_.size(); j++)
-        H_(0, rowIndex_ + j * M) = this->weights_(j);
+        H_(0, rowIndex_ + j * M_) = this->weights_(j);
     }
 
    public:
@@ -478,14 +473,15 @@ class Basis {
     ComponentDerivativeFunctor() {}
 
     /// Construct with row index
-    ComponentDerivativeFunctor(size_t N, size_t i, double x)
+    ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x)
-        : DerivativeFunctorBase(N, x), rowIndex_(i) {
+        : DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) {
       calculateJacobian(N);
     }
 
     /// Construct with row index and interval
-    ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
+    ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a,
-        : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
+                               double b)
+        : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) {
       calculateJacobian(N);
     }
     /// Calculate derivative of component rowIndex_ of F

gtsam/basis/BasisFactors.h

@@ -87,11 +87,10 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
  * measurement prediction function.
  *
  * @param BASIS: The basis class to use e.g. Chebyshev2
- * @param M: Size of the evaluated state vector.
  *
  * @ingroup basis
  */
-template <class BASIS, int M>
+template <class BASIS>
 class VectorEvaluationFactor
     : public FunctorizedFactor<Vector, ParameterMatrix> {
  private:
@@ -107,14 +106,14 @@ class VectorEvaluationFactor
    * polynomial.
    * @param z The measurement value.
    * @param model The noise model.
+   * @param M Size of the evaluated state vector.
    * @param N The degree of the polynomial.
    * @param x The point at which to evaluate the basis polynomial.
    */
   VectorEvaluationFactor(Key key, const Vector &z,
-                         const SharedNoiseModel &model, const size_t N,
+                         const SharedNoiseModel &model, const size_t M,
-                         double x)
+                         const size_t N, double x)
-      : Base(key, z, model,
+      : Base(key, z, model, typename BASIS::VectorEvaluationFunctor(M, N, x)) {}
-             typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}
 
   /**
    * @brief Construct a new VectorEvaluationFactor object.
@@ -123,16 +122,17 @@ class VectorEvaluationFactor
    * polynomial.
    * @param z The measurement value.
    * @param model The noise model.
+   * @param M Size of the evaluated state vector.
    * @param N The degree of the polynomial.
    * @param x The point at which to evaluate the basis polynomial.
    * @param a Lower bound for the polynomial.
    * @param b Upper bound for the polynomial.
    */
   VectorEvaluationFactor(Key key, const Vector &z,
-                         const SharedNoiseModel &model, const size_t N,
+                         const SharedNoiseModel &model, const size_t M,
-                         double x, double a, double b)
+                         const size_t N, double x, double a, double b)
       : Base(key, z, model,
-             typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
+             typename BASIS::VectorEvaluationFunctor(M, N, x, a, b)) {}
 
   virtual ~VectorEvaluationFactor() {}
 };
@@ -147,17 +147,15 @@ class VectorEvaluationFactor
  * indexed by `i`.
  *
  * @param BASIS: The basis class to use e.g. Chebyshev2
- * @param P: Size of the fixed-size vector.
  *
  * Example:
- *  VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
+ *  VectorComponentFactor<BASIS> controlPrior(key, measured, model,
  *                                            N, i, t, a, b);
  *  where N is the degree and i is the component index.
  *
  * @ingroup basis
  */
-// TODO(Varun) remove template P
+template <class BASIS>
-template <class BASIS, size_t P>
 class VectorComponentFactor
     : public FunctorizedFactor<double, ParameterMatrix> {
  private:
@@ -174,15 +172,16 @@ class VectorComponentFactor
    * @param z The scalar value at a specified index `i` of the full measurement
    * vector.
    * @param model The noise model.
+   * @param P Size of the fixed-size vector.
    * @param N The degree of the polynomial.
    * @param i The index for the evaluated vector to give us the desired scalar
    * value.
    * @param x The point at which to evaluate the basis polynomial.
    */
   VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
-                        const size_t N, size_t i, double x)
+                        const size_t P, const size_t N, size_t i, double x)
       : Base(key, z, model,
-             typename BASIS::template VectorComponentFunctor<P>(N, i, x)) {}
+             typename BASIS::VectorComponentFunctor(P, N, i, x)) {}
 
   /**
    * @brief Construct a new VectorComponentFactor object.
@@ -192,6 +191,7 @@ class VectorComponentFactor
    * @param z The scalar value at a specified index `i` of the full measurement
    * vector.
    * @param model The noise model.
+   * @param P Size of the fixed-size vector.
    * @param N The degree of the polynomial.
    * @param i The index for the evaluated vector to give us the desired scalar
    * value.
@@ -200,11 +200,10 @@ class VectorComponentFactor
    * @param b Upper bound for the polynomial.
    */
   VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
-                        const size_t N, size_t i, double x, double a, double b)
+                        const size_t P, const size_t N, size_t i, double x,
-      : Base(
+                        double a, double b)
-            key, z, model,
+      : Base(key, z, model,
-            typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
+             typename BASIS::VectorComponentFunctor(P, N, i, x, a, b)) {}
-  }
 
   virtual ~VectorComponentFactor() {}
 };
@@ -324,15 +323,13 @@ class DerivativeFactor
  * polynomial at a specified point `x` is equal to the vector value `z`.
  *
  * @param BASIS: The basis class to use e.g. Chebyshev2
- * @param M: Size of the evaluated state vector derivative.
  */
-//TODO(Varun) remove template M
+template <class BASIS>
-template <class BASIS, int M>
 class VectorDerivativeFactor
     : public FunctorizedFactor<Vector, ParameterMatrix> {
  private:
   using Base = FunctorizedFactor<Vector, ParameterMatrix>;
-  using Func = typename BASIS::template VectorDerivativeFunctor<M>;
+  using Func = typename BASIS::VectorDerivativeFunctor;
 
  public:
   VectorDerivativeFactor() {}
@@ -344,13 +341,14 @@ class VectorDerivativeFactor
    * polynomial.
    * @param z The measurement value.
    * @param model The noise model.
+   * @param M Size of the evaluated state vector derivative.
    * @param N The degree of the polynomial.
    * @param x The point at which to evaluate the basis polynomial.
    */
   VectorDerivativeFactor(Key key, const Vector &z,
-                         const SharedNoiseModel &model, const size_t N,
+                         const SharedNoiseModel &model, const size_t M,
-                         double x)
+                         const size_t N, double x)
-      : Base(key, z, model, Func(N, x)) {}
+      : Base(key, z, model, Func(M, N, x)) {}
 
   /**
    * @brief Construct a new VectorDerivativeFactor object.
@@ -359,15 +357,16 @@ class VectorDerivativeFactor
    * polynomial.
    * @param z The measurement value.
    * @param model The noise model.
+   * @param M Size of the evaluated state vector derivative.
    * @param N The degree of the polynomial.
    * @param x The point at which to evaluate the basis polynomial.
    * @param a Lower bound for the polynomial.
    * @param b Upper bound for the polynomial.
    */
   VectorDerivativeFactor(Key key, const Vector &z,
-                         const SharedNoiseModel &model, const size_t N,
+                         const SharedNoiseModel &model, const size_t M,
-                         double x, double a, double b)
+                         const size_t N, double x, double a, double b)
-      : Base(key, z, model, Func(N, x, a, b)) {}
+      : Base(key, z, model, Func(M, N, x, a, b)) {}
 
   virtual ~VectorDerivativeFactor() {}
 };
@@ -378,15 +377,13 @@ class VectorDerivativeFactor
  * vector-valued measurement `z`.
  *
  * @param BASIS: The basis class to use e.g. Chebyshev2
- * @param P: Size of the control component derivative.
  */
-// TODO(Varun) remove template P
+template <class BASIS>
-template <class BASIS, int P>
 class ComponentDerivativeFactor
     : public FunctorizedFactor<double, ParameterMatrix> {
  private:
   using Base = FunctorizedFactor<double, ParameterMatrix>;
-  using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
+  using Func = typename BASIS::ComponentDerivativeFunctor;
 
  public:
   ComponentDerivativeFactor() {}
@@ -399,15 +396,16 @@ class ComponentDerivativeFactor
    * @param z The scalar measurement value at a specific index `i` of the full
    * measurement vector.
    * @param model The degree of the polynomial.
+   * @param P: Size of the control component derivative.
    * @param N The degree of the polynomial.
    * @param i The index for the evaluated vector to give us the desired scalar
    * value.
    * @param x The point at which to evaluate the basis polynomial.
    */
   ComponentDerivativeFactor(Key key, const double &z,
-                            const SharedNoiseModel &model, const size_t N,
+                            const SharedNoiseModel &model, const size_t P,
-                            size_t i, double x)
+                            const size_t N, size_t i, double x)
-      : Base(key, z, model, Func(N, i, x)) {}
+      : Base(key, z, model, Func(P, N, i, x)) {}
 
   /**
    * @brief Construct a new ComponentDerivativeFactor object.
@@ -417,6 +415,7 @@ class ComponentDerivativeFactor
    * @param z The scalar measurement value at a specific index `i` of the full
    * measurement vector.
    * @param model The degree of the polynomial.
+   * @param P: Size of the control component derivative.
    * @param N The degree of the polynomial.
    * @param i The index for the evaluated vector to give us the desired scalar
    * value.
@@ -425,9 +424,10 @@ class ComponentDerivativeFactor
    * @param b Upper bound for the polynomial.
    */
   ComponentDerivativeFactor(Key key, const double &z,
-                            const SharedNoiseModel &model, const size_t N,
+                            const SharedNoiseModel &model, const size_t P,
-                            size_t i, double x, double a, double b)
+                            const size_t N, size_t i, double x, double a,
-      : Base(key, z, model, Func(N, i, x, a, b)) {}
+                            double b)
+      : Base(key, z, model, Func(P, N, i, x, a, b)) {}
 
   virtual ~ComponentDerivativeFactor() {}
 };

gtsam/basis/tests/testBasisFactors.cpp

@@ -80,7 +80,7 @@ TEST(BasisFactors, VectorEvaluationFactor) {
   const Vector measured = Vector::Zero(M);
 
   auto model = Isotropic::Sigma(M, 1.0);
-  VectorEvaluationFactor<Chebyshev2, M> factor(key, measured, model, N, 0);
+  VectorEvaluationFactor<Chebyshev2> factor(key, measured, model, M, N, 0);
 
   NonlinearFactorGraph graph;
   graph.add(factor);
@@ -106,7 +106,7 @@ TEST(BasisFactors, Print) {
   const Vector measured = Vector::Ones(M) * 42;
 
   auto model = Isotropic::Sigma(M, 1.0);
-  VectorEvaluationFactor<Chebyshev2, M> factor(key, measured, model, N, 0);
+  VectorEvaluationFactor<Chebyshev2> factor(key, measured, model, M, N, 0);
 
   std::string expected =
       "  keys = { X0 }\n"
@@ -127,7 +127,7 @@ TEST(BasisFactors, VectorComponentFactor) {
   const size_t i = 2;
   const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
   auto model = Isotropic::Sigma(1, 1.0);
-  VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i, t, a,
+  VectorComponentFactor<Chebyshev2> factor(key, measured, model, P, N, i, t, a,
                                            b);
 
   NonlinearFactorGraph graph;
@@ -180,7 +180,7 @@ TEST(BasisFactors, VecDerivativePrior) {
 
   const Vector measured = Vector::Zero(M);
   auto model = Isotropic::Sigma(M, 1.0);
-  VectorDerivativeFactor<Chebyshev2, M> vecDPrior(key, measured, model, N, 0);
+  VectorDerivativeFactor<Chebyshev2> vecDPrior(key, measured, model, M, N, 0);
 
   NonlinearFactorGraph graph;
   graph.add(vecDPrior);
@@ -205,7 +205,7 @@ TEST(BasisFactors, ComponentDerivativeFactor) {
 
   double measured = 0;
   auto model = Isotropic::Sigma(1, 1.0);
-  ComponentDerivativeFactor<Chebyshev2, M> controlDPrior(key, measured, model,
+  ComponentDerivativeFactor<Chebyshev2> controlDPrior(key, measured, model, M,
                                                       N, 0, 0);
 
   NonlinearFactorGraph graph;

gtsam/basis/tests/testChebyshev2.cpp

@@ -116,12 +116,12 @@ TEST(Chebyshev2, InterpolateVector) {
   expected << t, 0;
   Eigen::Matrix<double, /*2x2N*/ -1, -1> actualH(2, 2 * N);
 
-  Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b);
+  Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b);
   EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
 
   // Check derivative
   std::function<Vector2(ParameterMatrix)> f =
-      std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx,
+      std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
                 std::placeholders::_1, nullptr);
   Matrix numericalH =
       numericalDerivative11<Vector2, ParameterMatrix, 2 * N>(f, X);
@@ -413,8 +413,8 @@ TEST(Chebyshev2, Derivative6_03) {
 TEST(Chebyshev2, VectorDerivativeFunctor) {
   const size_t N = 3, M = 2;
   const double x = 0.2;
-  using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
+  using VecD = Chebyshev2::VectorDerivativeFunctor;
-  VecD fx(N, x, 0, 3);
+  VecD fx(M, N, x, 0, 3);
   ParameterMatrix X(M, N);
   Matrix actualH(M, M * N);
   EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
@@ -429,7 +429,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
 // Test VectorDerivativeFunctor with polynomial function
 TEST(Chebyshev2, VectorDerivativeFunctor2) {
   const size_t N = 64, M = 1, T = 15;
-  using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
+  using VecD = Chebyshev2::VectorDerivativeFunctor;
 
   const Vector points = Chebyshev2::Points(N, 0, T);
 
@@ -443,14 +443,14 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
   // Evaluate the derivative at the chebyshev points using
   // VectorDerivativeFunctor.
   for (size_t i = 0; i < N; ++i) {
-    VecD d(N, points(i), 0, T);
+    VecD d(M, N, points(i), 0, T);
     Vector1 Dx = d(X);
     EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6);
   }
 
   // Test Jacobian at the first chebyshev point.
   Matrix actualH(M, M * N);
-  VecD vecd(N, points(0), 0, T);
+  VecD vecd(M, N, points(0), 0, T);
   vecd(X, actualH);
   Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix, M * N>(
       std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
@@ -462,9 +462,9 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
 TEST(Chebyshev2, ComponentDerivativeFunctor) {
   const size_t N = 6, M = 2;
   const double x = 0.2;
-  using CompFunc = Chebyshev2::ComponentDerivativeFunctor<M>;
+  using CompFunc = Chebyshev2::ComponentDerivativeFunctor;
   size_t row = 1;
-  CompFunc fx(N, row, x, 0, 3);
+  CompFunc fx(M, N, row, x, 0, 3);
   ParameterMatrix X(M, N);
   Matrix actualH(1, M * N);
   EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);

gtsam/basis/tests/testFourier.cpp

@@ -180,13 +180,13 @@ TEST(Basis, Derivative7) {
 
 //******************************************************************************
 TEST(Basis, VecDerivativeFunctor) {
-  using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>;
+  using DotShape = typename FourierBasis::VectorDerivativeFunctor;
   const size_t N = 3;
 
   // MATLAB example, Dec 27 2019, commit 014eded5
   double h = 2 * M_PI / 16;
   Vector2 dotShape(0.5556, -0.8315);  // at h/2
-  DotShape dotShapeFunction(N, h / 2);
+  DotShape dotShapeFunction(2, N, h / 2);
   Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
                            .finished()
                            .transpose();