Merge pull request #1537 from borglab/basis-test

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

@@ -20,7 +20,6 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/OptionalJacobian.h>
-#include <gtsam/basis/ParameterMatrix.h>
 
 #include <iostream>
 
@@ -81,16 +80,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
@@ -169,18 +159,18 @@ class Basis {
   };
 
   /**
-   * VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
+   * VectorEvaluationFunctor at a given x, applied to a parameter Matrix.
    * This functor is used to evaluate a parameterized function at a given scalar
    * 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 +180,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,26 +190,27 @@ 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<M>& P,
+    Vector apply(const Matrix& P,
-                  OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
+                 OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
       if (H) *H = H_;
       return P.matrix() * this->weights_.transpose();
     }
 
     /// c++ sugar
-    VectorM operator()(const ParameterMatrix<M>& P,
+    Vector operator()(const Matrix& P,
-                       OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
+                      OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
       return apply(P, H);
     }
   };
@@ -231,13 +222,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 +240,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,33 +250,34 @@ 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);
     }
 
     /// Calculate component of component rowIndex_ of P
-    double apply(const ParameterMatrix<M>& P,
+    double apply(const Matrix& P,
                  OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
       if (H) *H = H_;
       return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
     }
 
     /// c++ sugar
-    double operator()(const ParameterMatrix<M>& P,
+    double operator()(const Matrix& P,
                       OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
       return apply(P, H);
     }
   };
 
   /**
-   * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
+   * Manifold EvaluationFunctor at a given x, applied to a parameter Matrix.
    * This functor is used to evaluate a parameterized function at a given scalar
    * 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.
@@ -297,25 +290,23 @@ 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<M>& P,
+    T apply(const Matrix& P, OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
-            OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
       // Interpolate the M-dimensional vector to yield a vector in tangent space
       Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
 
@@ -333,7 +324,7 @@ class Basis {
     }
 
     /// c++ sugar
-    T operator()(const ParameterMatrix<M>& P,
+    T operator()(const Matrix& P,
                  OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
       return apply(P, H);  // might call apply in derived
     }
@@ -389,20 +380,20 @@ class Basis {
   };
 
   /**
-   * VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
+   * VectorDerivativeFunctor at a given x, applied to a parameter Matrix.
    *
    * This functor is used to evaluate the derivatives of a parameterized
    * function at a given scalar value x. When given a specific M*N parameters,
    * 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 +403,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,25 +413,25 @@ 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<M>& P,
+    Vector apply(const Matrix& P,
-                  OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
+                 OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
       if (H) *H = H_;
       return P.matrix() * this->weights_.transpose();
     }
     /// c++ sugar
-    VectorM operator()(
+    Vector operator()(const Matrix& P,
-        const ParameterMatrix<M>& P,
+                      OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
-        OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
       return apply(P, H);
     }
   };
@@ -452,13 +443,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`.
@@ -469,9 +461,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:
@@ -479,29 +471,29 @@ 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
-    double apply(const ParameterMatrix<M>& P,
+    double apply(const Matrix& P,
                  OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
       if (H) *H = H_;
       return P.row(rowIndex_) * this->weights_.transpose();
     }
     /// c++ sugar
-    double operator()(const ParameterMatrix<M>& P,
+    double operator()(const Matrix& P,
                       OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
       return apply(P, H);
     }
   };
-
 };
 
 }  // namespace gtsam

gtsam/basis/BasisFactors.h

@@ -75,7 +75,7 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
 };
 
 /**
- * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix
+ * Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix
  * of size (M, N) is equal to a vector-valued measurement at the same point,
  when
  * using a pseudo-spectral parameterization.
@@ -87,15 +87,13 @@ 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
+class VectorEvaluationFactor : public FunctorizedFactor<Vector, Matrix> {
-    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
  private:
-  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
+  using Base = FunctorizedFactor<Vector, Matrix>;
 
  public:
   VectorEvaluationFactor() {}
@@ -103,42 +101,43 @@ class VectorEvaluationFactor
   /**
    * @brief Construct a new VectorEvaluationFactor object.
    *
-   * @param key The key to the ParameterMatrix object used to represent the
+   * @param key The key to the parameter Matrix object used to represent the
    * 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.
    *
-   * @param key The key to the ParameterMatrix object used to represent the
+   * @param key The key to the parameter Matrix object used to represent the
    * 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() {}
 };
 
 /**
- * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix
+ * Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix
  * of size (P, N) is equal to specified measurement at the same point, when
  * using a pseudo-spectral parameterization.
  *
@@ -147,20 +146,18 @@ 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);
+ *                                            N, i, t, a, b);
  *  where N is the degree and i is the component index.
  *
  * @ingroup basis
  */
-template <class BASIS, size_t P>
+template <class BASIS>
-class VectorComponentFactor
+class VectorComponentFactor : public FunctorizedFactor<double, Matrix> {
-    : public FunctorizedFactor<double, ParameterMatrix<P>> {
  private:
-  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
+  using Base = FunctorizedFactor<double, Matrix>;
 
  public:
   VectorComponentFactor() {}
@@ -168,29 +165,31 @@ class VectorComponentFactor
   /**
    * @brief Construct a new VectorComponentFactor object.
    *
-   * @param key The key to the ParameterMatrix object used to represent the
+   * @param key The key to the parameter Matrix object used to represent the
    * polynomial.
    * @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.
    *
-   * @param key The key to the ParameterMatrix object used to represent the
+   * @param key The key to the parameter Matrix object used to represent the
    * polynomial.
    * @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.
@@ -199,11 +198,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() {}
 };
@@ -226,10 +224,9 @@ class VectorComponentFactor
  * where `x` is the value (e.g. timestep) at which the rotation was evaluated.
  */
 template <class BASIS, typename T>
-class ManifoldEvaluationFactor
+class ManifoldEvaluationFactor : public FunctorizedFactor<T, Matrix> {
-    : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
  private:
-  using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
+  using Base = FunctorizedFactor<T, Matrix>;
 
  public:
   ManifoldEvaluationFactor() {}
@@ -289,7 +286,7 @@ class DerivativeFactor
   /**
    * @brief Construct a new DerivativeFactor object.
    *
-   * @param key The key to the ParameterMatrix which represents the basis
+   * @param key The key to the parameter Matrix which represents the basis
    * polynomial.
    * @param z The measurement value.
    * @param model The noise model.
@@ -303,7 +300,7 @@ class DerivativeFactor
   /**
    * @brief Construct a new DerivativeFactor object.
    *
-   * @param key The key to the ParameterMatrix which represents the basis
+   * @param key The key to the parameter Matrix which represents the basis
    * polynomial.
    * @param z The measurement value.
    * @param model The noise model.
@@ -324,14 +321,12 @@ 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.
  */
-template <class BASIS, int M>
+template <class BASIS>
-class VectorDerivativeFactor
+class VectorDerivativeFactor : public FunctorizedFactor<Vector, Matrix> {
-    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
  private:
-  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
+  using Base = FunctorizedFactor<Vector, Matrix>;
-  using Func = typename BASIS::template VectorDerivativeFunctor<M>;
+  using Func = typename BASIS::VectorDerivativeFunctor;
 
  public:
   VectorDerivativeFactor() {}
@@ -339,34 +334,36 @@ class VectorDerivativeFactor
   /**
    * @brief Construct a new VectorDerivativeFactor object.
    *
-   * @param key The key to the ParameterMatrix which represents the basis
+   * @param key The key to the parameter Matrix which represents the basis
    * 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.
    *
-   * @param key The key to the ParameterMatrix which represents the basis
+   * @param key The key to the parameter Matrix which represents the basis
    * 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() {}
 };
@@ -377,14 +374,12 @@ class VectorDerivativeFactor
  * vector-valued measurement `z`.
  *
  * @param BASIS: The basis class to use e.g. Chebyshev2
- * @param P: Size of the control component derivative.
  */
-template <class BASIS, int P>
+template <class BASIS>
-class ComponentDerivativeFactor
+class ComponentDerivativeFactor : public FunctorizedFactor<double, Matrix> {
-    : public FunctorizedFactor<double, ParameterMatrix<P>> {
  private:
-  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
+  using Base = FunctorizedFactor<double, Matrix>;
-  using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
+  using Func = typename BASIS::ComponentDerivativeFunctor;
 
  public:
   ComponentDerivativeFactor() {}
@@ -392,29 +387,31 @@ class ComponentDerivativeFactor
   /**
    * @brief Construct a new ComponentDerivativeFactor object.
    *
-   * @param key The key to the ParameterMatrix which represents the basis
+   * @param key The key to the parameter Matrix which represents the basis
    * polynomial.
    * @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.
    *
-   * @param key The key to the ParameterMatrix which represents the basis
+   * @param key The key to the parameter Matrix which represents the basis
    * polynomial.
    * @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.
@@ -423,9 +420,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/ParameterMatrix.h

@@ -1,215 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 ParameterMatrix.h
- * @brief Define ParameterMatrix class which is used to store values at
- * interpolation points.
- * @author Varun Agrawal, Frank Dellaert
- * @date September 21, 2020
- */
-
-#pragma once
-
-#include <gtsam/base/Matrix.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/VectorSpace.h>
-
-#include <iostream>
-
-namespace gtsam {
-
-/**
- * A matrix abstraction of MxN values at the Basis points.
- * This class serves as a wrapper over an Eigen matrix.
- * @tparam M: The dimension of the type you wish to evaluate.
- * @param N: the number of Basis points (e.g. Chebyshev points of the second
- * kind).
- */
-template <int M>
-class ParameterMatrix {
-  using MatrixType = Eigen::Matrix<double, M, -1>;
-
- private:
-  MatrixType matrix_;
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  enum { dimension = Eigen::Dynamic };
-
-  /**
-   * Create ParameterMatrix using the number of basis points.
-   * @param N: The number of basis points (the columns).
-   */
-  ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
-
-  /**
-   * Create ParameterMatrix from an MxN Eigen Matrix.
-   * @param matrix: An Eigen matrix used to initialze the ParameterMatrix.
-   */
-  ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
-
-  /// Get the number of rows.
-  size_t rows() const { return matrix_.rows(); }
-
-  /// Get the number of columns.
-  size_t cols() const { return matrix_.cols(); }
-
-  /// Get the underlying matrix.
-  MatrixType matrix() const { return matrix_; }
-
-  /// Return the tranpose of the underlying matrix.
-  Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); }
-
-  /**
-   * Get the matrix row specified by `index`.
-   * @param index: The row index to retrieve.
-   */
-  Eigen::Matrix<double, 1, -1> row(size_t index) const {
-    return matrix_.row(index);
-  }
-
-  /**
-   * Set the matrix row specified by `index`.
-   * @param index: The row index to set.
-   */
-  auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> {
-    return matrix_.row(index);
-  }
-
-  /**
-   * Get the matrix column specified by `index`.
-   * @param index: The column index to retrieve.
-   */
-  Eigen::Matrix<double, M, 1> col(size_t index) const {
-    return matrix_.col(index);
-  }
-
-  /**
-   * Set the matrix column specified by `index`.
-   * @param index: The column index to set.
-   */
-  auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> {
-    return matrix_.col(index);
-  }
-
-  /**
-   * Set all matrix coefficients to zero.
-   */
-  void setZero() { matrix_.setZero(); }
-
-  /**
-   * Add a ParameterMatrix to another.
-   * @param other: ParameterMatrix to add.
-   */
-  ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const {
-    return ParameterMatrix<M>(matrix_ + other.matrix());
-  }
-
-  /**
-   * Add a MxN-sized vector to the ParameterMatrix.
-   * @param other: Vector which is reshaped and added.
-   */
-  ParameterMatrix<M> operator+(
-      const Eigen::Matrix<double, -1, 1>& other) const {
-    // This form avoids a deep copy and instead typecasts `other`.
-    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
-    return ParameterMatrix<M>(matrix_ + other_);
-  }
-
-  /**
-   * Subtract a ParameterMatrix from another.
-   * @param other: ParameterMatrix to subtract.
-   */
-  ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
-    return ParameterMatrix<M>(matrix_ - other.matrix());
-  }
-
-  /**
-   * Subtract a MxN-sized vector from the ParameterMatrix.
-   * @param other: Vector which is reshaped and subracted.
-   */
-  ParameterMatrix<M> operator-(
-      const Eigen::Matrix<double, -1, 1>& other) const {
-    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
-    return ParameterMatrix<M>(matrix_ - other_);
-  }
-
-  /**
-   * Multiply ParameterMatrix with an Eigen matrix.
-   * @param other: Eigen matrix which should be multiplication compatible with
-   * the ParameterMatrix.
-   */
-  MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const {
-    return matrix_ * other;
-  }
-
-  /// @name Vector Space requirements
-  /// @{
-
-  /**
-   * Print the ParameterMatrix.
-   * @param s: The prepend string to add more contextual info.
-   */
-  void print(const std::string& s = "") const {
-    std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl;
-  }
-
-  /**
-   * Check for equality up to absolute tolerance.
-   * @param other: The ParameterMatrix to check equality with.
-   * @param tol: The absolute tolerance threshold.
-   */
-  bool equals(const ParameterMatrix<M>& other, double tol = 1e-8) const {
-    return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
-  }
-
-  /// Returns dimensionality of the tangent space
-  inline size_t dim() const { return matrix_.size(); }
-
-  /// Convert to vector form, is done row-wise
-  inline Vector vector() const {
-    using RowMajor = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;
-    Vector result(matrix_.size());
-    Eigen::Map<RowMajor>(&result(0), rows(), cols()) = matrix_;
-    return result;
-  }
-
-  /** Identity function to satisfy VectorSpace traits.
-   *
-   * NOTE: The size at compile time is unknown so this identity is zero
-   * length and thus not valid.
-   */
-  inline static ParameterMatrix Identity() {
-    // throw std::runtime_error(
-    //     "ParameterMatrix::Identity(): Don't use this function");
-    return ParameterMatrix(0);
-  }
-
-  /// @}
-};
-
-// traits for ParameterMatrix
-template <int M>
-struct traits<ParameterMatrix<M>>
-    : public internal::VectorSpace<ParameterMatrix<M>> {};
-
-/* ************************************************************************* */
-// Stream operator that takes a ParameterMatrix. Used for printing.
-template <int M>
-inline std::ostream& operator<<(std::ostream& os,
-                                const ParameterMatrix<M>& parameterMatrix) {
-  os << parameterMatrix.matrix();
-  return os;
-}
-
-}  // namespace gtsam

gtsam/basis/basis.i

@@ -46,18 +46,6 @@ class Chebyshev2 {
   static Matrix DifferentiationMatrix(size_t N, double a, double b);
 };
 
-#include <gtsam/basis/ParameterMatrix.h>
-
-template <M = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}>
-class ParameterMatrix {
-  ParameterMatrix(const size_t N);
-  ParameterMatrix(const Matrix& matrix);
-
-  Matrix matrix() const;
-
-  void print(const string& s = "") const;
-};
-
 #include <gtsam/basis/BasisFactors.h>
 
 template <BASIS = {gtsam::Chebyshev2, gtsam::Chebyshev1Basis,
@@ -72,45 +60,36 @@ virtual class EvaluationFactor : gtsam::NoiseModelFactor {
                    double x, double a, double b);
 };
 
-template <BASIS, M>
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
 virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
   VectorEvaluationFactor();
   VectorEvaluationFactor(gtsam::Key key, const Vector& z,
-                         const gtsam::noiseModel::Base* model, const size_t N,
+                         const gtsam::noiseModel::Base* model, const size_t M,
-                         double x);
+                         const size_t N, double x);
   VectorEvaluationFactor(gtsam::Key key, const Vector& z,
-                         const gtsam::noiseModel::Base* model, const size_t N,
+                         const gtsam::noiseModel::Base* model, const size_t M,
-                         double x, double a, double b);
+                         const size_t N, double x, double a, double b);
 };
 
-// TODO(Varun) Better way to support arbitrary dimensions?
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
-// Especially if users mainly do `pip install gtsam` for the Python wrapper.
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
-typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 3>
-    VectorEvaluationFactorChebyshev2D3;
-typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 4>
-    VectorEvaluationFactorChebyshev2D4;
-typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 12>
-    VectorEvaluationFactorChebyshev2D12;
-
-template <BASIS, P>
 virtual class VectorComponentFactor : gtsam::NoiseModelFactor {
   VectorComponentFactor();
   VectorComponentFactor(gtsam::Key key, const double z,
-                        const gtsam::noiseModel::Base* model, const size_t N,
+                        const gtsam::noiseModel::Base* model, const size_t M,
-                        size_t i, double x);
+                        const size_t N, size_t i, double x);
   VectorComponentFactor(gtsam::Key key, const double z,
-                        const gtsam::noiseModel::Base* model, const size_t N,
+                        const gtsam::noiseModel::Base* model, const size_t M,
-                        size_t i, double x, double a, double b);
+                        const size_t N, size_t i, double x, double a, double b);
 };
 
-typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 3>
+#include <gtsam/geometry/Pose2.h>
-    VectorComponentFactorChebyshev2D3;
+#include <gtsam/geometry/Pose3.h>
-typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 4>
-    VectorComponentFactorChebyshev2D4;
-typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 12>
-    VectorComponentFactorChebyshev2D12;
 
-template <BASIS, T>
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2},
+          T = {gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
 virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
   ManifoldEvaluationFactor();
   ManifoldEvaluationFactor(gtsam::Key key, const T& z,
@@ -121,8 +100,42 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
                            double x, double a, double b);
 };
 
-// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
-// `ComponentDerivativeFactor`
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
+virtual class DerivativeFactor : gtsam::NoiseModelFactor {
+  DerivativeFactor();
+  DerivativeFactor(gtsam::Key key, const double z,
+                   const gtsam::noiseModel::Base* model, const size_t N,
+                   double x);
+  DerivativeFactor(gtsam::Key key, const double z,
+                   const gtsam::noiseModel::Base* model, const size_t N,
+                   double x, double a, double b);
+};
+
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
+virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor {
+  VectorDerivativeFactor();
+  VectorDerivativeFactor(gtsam::Key key, const Vector& z,
+                         const gtsam::noiseModel::Base* model, const size_t M,
+                         const size_t N, double x);
+  VectorDerivativeFactor(gtsam::Key key, const Vector& z,
+                         const gtsam::noiseModel::Base* model, const size_t M,
+                         const size_t N, double x, double a, double b);
+};
+
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
+virtual class ComponentDerivativeFactor : gtsam::NoiseModelFactor {
+  ComponentDerivativeFactor();
+  ComponentDerivativeFactor(gtsam::Key key, const double z,
+                            const gtsam::noiseModel::Base* model,
+                            const size_t P, const size_t N, size_t i, double x);
+  ComponentDerivativeFactor(gtsam::Key key, const double z,
+                            const gtsam::noiseModel::Base* model,
+                            const size_t P, const size_t N, size_t i, double x,
+                            double a, double b);
+};
 
 #include <gtsam/basis/FitBasis.h>
 template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,

gtsam/basis/tests/testBasisFactors.cpp

@@ -17,30 +17,28 @@
  * @brief unit tests for factors in BasisFactors.h
  */
 
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/basis/Basis.h>
 #include <gtsam/basis/BasisFactors.h>
 #include <gtsam/basis/Chebyshev2.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/FunctorizedFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/factorTesting.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/base/Vector.h>
 
-#include <CppUnitLite/TestHarness.h>
-
-using gtsam::noiseModel::Isotropic;
-using gtsam::Pose2;
-using gtsam::Vector;
-using gtsam::Values;
 using gtsam::Chebyshev2;
-using gtsam::ParameterMatrix;
-using gtsam::LevenbergMarquardtParams;
 using gtsam::LevenbergMarquardtOptimizer;
+using gtsam::LevenbergMarquardtParams;
 using gtsam::NonlinearFactorGraph;
 using gtsam::NonlinearOptimizerParams;
+using gtsam::Pose2;
+using gtsam::Values;
+using gtsam::Vector;
+using gtsam::noiseModel::Isotropic;
 
 constexpr size_t N = 2;
 
@@ -81,15 +79,15 @@ 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);
 
-  ParameterMatrix<M> stateMatrix(N);
+  gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
 
   Values initial;
-  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+  initial.insert<gtsam::Matrix>(key, stateMatrix);
 
   LevenbergMarquardtParams parameters;
   parameters.setMaxIterations(20);
@@ -107,7 +105,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"
@@ -128,16 +126,16 @@ 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,
+  VectorComponentFactor<Chebyshev2> factor(key, measured, model, P, N, i, t, a,
-                                                    t, a, b);
+                                           b);
 
   NonlinearFactorGraph graph;
   graph.add(factor);
 
-  ParameterMatrix<P> stateMatrix(N);
+  gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N);
 
   Values initial;
-  initial.insert<ParameterMatrix<P>>(key, stateMatrix);
+  initial.insert<gtsam::Matrix>(key, stateMatrix);
 
   LevenbergMarquardtParams parameters;
   parameters.setMaxIterations(20);
@@ -153,16 +151,16 @@ TEST(BasisFactors, ManifoldEvaluationFactor) {
   const Pose2 measured;
   const double t = 3.0, a = 2.0, b = 4.0;
   auto model = Isotropic::Sigma(3, 1.0);
-  ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N,
+  ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N, t,
-                                                     t, a, b);
+                                                     a, b);
 
   NonlinearFactorGraph graph;
   graph.add(factor);
 
-  ParameterMatrix<3> stateMatrix(N);
+  gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N);
 
   Values initial;
-  initial.insert<ParameterMatrix<3>>(key, stateMatrix);
+  initial.insert<gtsam::Matrix>(key, stateMatrix);
 
   LevenbergMarquardtParams parameters;
   parameters.setMaxIterations(20);
@@ -170,6 +168,8 @@ TEST(BasisFactors, ManifoldEvaluationFactor) {
       LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
 
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, initial, 1e-7, 1e-5);
 }
 
 //******************************************************************************
@@ -179,15 +179,15 @@ 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);
 
-  ParameterMatrix<M> stateMatrix(N);
+  gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
 
   Values initial;
-  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+  initial.insert<gtsam::Matrix>(key, stateMatrix);
 
   LevenbergMarquardtParams parameters;
   parameters.setMaxIterations(20);
@@ -204,15 +204,15 @@ 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);
+                                                      N, 0, 0);
 
   NonlinearFactorGraph graph;
   graph.add(controlDPrior);
 
   Values initial;
-  ParameterMatrix<M> stateMatrix(N);
+  gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
-  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+  initial.insert<gtsam::Matrix>(key, stateMatrix);
 
   LevenbergMarquardtParams parameters;
   parameters.setMaxIterations(20);

gtsam/basis/tests/testChebyshev2.cpp

@@ -17,14 +17,15 @@
  *        methods
  */
 
-#include <cstddef>
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/basis/Chebyshev2.h>
 #include <gtsam/basis/FitBasis.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/factorTesting.h>
-#include <gtsam/base/Testable.h>
 
-#include <CppUnitLite/TestHarness.h>
+#include <cstddef>
 #include <functional>
 
 using namespace std;
@@ -107,7 +108,7 @@ TEST(Chebyshev2, InterpolateVector) {
   double t = 30, a = 0, b = 100;
   const size_t N = 3;
   // Create 2x3 matrix with Vectors at Chebyshev points
-  ParameterMatrix<2> X(N);
+  Matrix X = Matrix::Zero(2, N);
   X.row(0) = Chebyshev2::Points(N, a, b);  // slope 1 ramp
 
   // Check value
@@ -115,14 +116,15 @@ 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<2>)> f = std::bind(
+  std::function<Vector2(Matrix)> f =
-      &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, std::placeholders::_1, nullptr);
+      std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
+                std::placeholders::_1, nullptr);
   Matrix numericalH =
-      numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
+      numericalDerivative11<Vector2, Matrix, 2 * N>(f, X);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
@@ -131,25 +133,66 @@ TEST(Chebyshev2, InterpolateVector) {
 TEST(Chebyshev2, InterpolatePose2) {
   double t = 30, a = 0, b = 100;
 
-  ParameterMatrix<3> X(N);
+  Matrix X(3, N);
   X.row(0) = Chebyshev2::Points(N, a, b);  // slope 1 ramp
   X.row(1) = Vector::Zero(N);
   X.row(2) = 0.1 * Vector::Ones(N);
 
   Vector xi(3);
   xi << t, 0, 0.1;
+  Eigen::Matrix<double, /*3x3N*/ -1, -1> actualH(3, 3 * N);
+
   Chebyshev2::ManifoldEvaluationFunctor<Pose2> fx(N, t, a, b);
   // We use xi as canonical coordinates via exponential map
   Pose2 expected = Pose2::ChartAtOrigin::Retract(xi);
-  EXPECT(assert_equal(expected, fx(X)));
+  EXPECT(assert_equal(expected, fx(X, actualH)));
+
+  // Check derivative
+  std::function<Pose2(Matrix)> f =
+      std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
+                std::placeholders::_1, nullptr);
+  Matrix numericalH =
+      numericalDerivative11<Pose2, Matrix, 3 * N>(f, X);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
+#ifdef GTSAM_POSE3_EXPMAP
+//******************************************************************************
+// Interpolating poses using the exponential map
+TEST(Chebyshev2, InterpolatePose3) {
+  double a = 10, b = 100;
+  double t = Chebyshev2::Points(N, a, b)(11);
+
+  Rot3 R = Rot3::Ypr(-2.21366492e-05, -9.35353636e-03, -5.90463598e-04);
+  Pose3 pose(R, Point3(1, 2, 3));
+
+  Vector6 xi = Pose3::ChartAtOrigin::Local(pose);
+  Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N);
+
+  Matrix X = Matrix::Zero(6, N);
+  X.col(11) = xi;
+
+  Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b);
+  // We use xi as canonical coordinates via exponential map
+  Pose3 expected = Pose3::ChartAtOrigin::Retract(xi);
+  EXPECT(assert_equal(expected, fx(X, actualH)));
+
+  // Check derivative
+  std::function<Pose3(Matrix)> f =
+      std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
+                std::placeholders::_1, nullptr);
+  Matrix numericalH =
+      numericalDerivative11<Pose3, Matrix, 6 * N>(f, X);
+  EXPECT(assert_equal(numericalH, actualH, 1e-8));
+}
+#endif
+
 //******************************************************************************
 TEST(Chebyshev2, Decomposition) {
   // Create example sequence
   Sequence sequence;
   for (size_t i = 0; i < 16; i++) {
-    double x = (1.0/ 16)*i - 0.99, y = x;
+    double x = (1.0 / 16) * i - 0.99, y = x;
     sequence[x] = y;
   }
 
@@ -370,14 +413,14 @@ 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<M> X(N);
+  Matrix X = Matrix::Zero(M, N);
   Matrix actualH(M, M * N);
   EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
 
   // Test Jacobian
-  Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>(
+  Matrix expectedH = numericalDerivative11<Vector2, Matrix, M * N>(
       std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
   EXPECT(assert_equal(expectedH, actualH, 1e-7));
 }
@@ -386,30 +429,29 @@ 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);
 
-  // Assign the parameter matrix
+  // Assign the parameter matrix 1xN
-  Vector values(N);
+  Matrix X(1, N);
   for (size_t i = 0; i < N; ++i) {
-    values(i) = f(points(i));
+    X(i) = f(points(i));
   }
-  ParameterMatrix<M> X(values);
 
   // 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>, M * N>(
+  Matrix expectedH = numericalDerivative11<Vector1, Matrix, M * N>(
       std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
   EXPECT(assert_equal(expectedH, actualH, 1e-6));
 }
@@ -419,14 +461,14 @@ 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<M> X(N);
+  Matrix X = Matrix::Zero(M, N);
   Matrix actualH(1, M * N);
   EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
 
-  Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>(
+  Matrix expectedH = numericalDerivative11<double, Matrix, M * N>(
       std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
   EXPECT(assert_equal(expectedH, actualH, 1e-7));
 }

gtsam/basis/tests/testFourier.cpp

@@ -180,17 +180,16 @@ 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)
+  Matrix theta = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
-                           .finished()
+                     .finished()
-                           .transpose();
+                     .transpose();
-  ParameterMatrix<2> theta(theta_mat);
   EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
 }
 

gtsam/basis/tests/testParameterMatrix.cpp

@@ -1,145 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testParameterMatrix.cpp
- * @date Sep 22, 2020
- * @author Varun Agrawal, Frank Dellaert
- * @brief Unit tests for ParameterMatrix.h
- */
-
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/basis/BasisFactors.h>
-#include <gtsam/basis/Chebyshev2.h>
-#include <gtsam/basis/ParameterMatrix.h>
-#include <gtsam/inference/Symbol.h>
-
-using namespace std;
-using namespace gtsam;
-
-using Parameters = Chebyshev2::Parameters;
-
-const size_t M = 2, N = 5;
-
-//******************************************************************************
-TEST(ParameterMatrix, Constructor) {
-  ParameterMatrix<2> actual1(3);
-  ParameterMatrix<2> expected1(Matrix::Zero(2, 3));
-  EXPECT(assert_equal(expected1, actual1));
-
-  ParameterMatrix<2> actual2(Matrix::Ones(2, 3));
-  ParameterMatrix<2> expected2(Matrix::Ones(2, 3));
-  EXPECT(assert_equal(expected2, actual2));
-  EXPECT(assert_equal(expected2.matrix(), actual2.matrix()));
-}
-
-//******************************************************************************
-TEST(ParameterMatrix, Dimensions) {
-  ParameterMatrix<M> params(N);
-  EXPECT_LONGS_EQUAL(params.rows(), M);
-  EXPECT_LONGS_EQUAL(params.cols(), N);
-  EXPECT_LONGS_EQUAL(params.dim(), M * N);
-}
-
-//******************************************************************************
-TEST(ParameterMatrix, Getters) {
-  ParameterMatrix<M> params(N);
-
-  Matrix expectedMatrix = Matrix::Zero(2, 5);
-  EXPECT(assert_equal(expectedMatrix, params.matrix()));
-
-  Matrix expectedMatrixTranspose = Matrix::Zero(5, 2);
-  EXPECT(assert_equal(expectedMatrixTranspose, params.transpose()));
-
-  ParameterMatrix<M> p2(Matrix::Ones(M, N));
-  Vector expectedRowVector = Vector::Ones(N);
-  for (size_t r = 0; r < M; ++r) {
-    EXPECT(assert_equal(p2.row(r), expectedRowVector));
-  }
-
-  ParameterMatrix<M> p3(2 * Matrix::Ones(M, N));
-  Vector expectedColVector = 2 * Vector::Ones(M);
-  for (size_t c = 0; c < M; ++c) {
-    EXPECT(assert_equal(p3.col(c), expectedColVector));
-  }
-}
-
-//******************************************************************************
-TEST(ParameterMatrix, Setters) {
-  ParameterMatrix<M> params(Matrix::Zero(M, N));
-  Matrix expected = Matrix::Zero(M, N);
-
-  // row
-  params.row(0) = Vector::Ones(N);
-  expected.row(0) = Vector::Ones(N);
-  EXPECT(assert_equal(expected, params.matrix()));
-
-  // col
-  params.col(2) = Vector::Ones(M);
-  expected.col(2) = Vector::Ones(M);
-
-  EXPECT(assert_equal(expected, params.matrix()));
-
-  // setZero
-  params.setZero();
-  expected.setZero();
-  EXPECT(assert_equal(expected, params.matrix()));
-}
-
-//******************************************************************************
-TEST(ParameterMatrix, Addition) {
-  ParameterMatrix<M> params(Matrix::Ones(M, N));
-  ParameterMatrix<M> expected(2 * Matrix::Ones(M, N));
-
-  // Add vector
-  EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
-  // Add another ParameterMatrix
-  ParameterMatrix<M> actual = params + ParameterMatrix<M>(Matrix::Ones(M, N));
-  EXPECT(assert_equal(expected, actual));
-}
-
-//******************************************************************************
-TEST(ParameterMatrix, Subtraction) {
-  ParameterMatrix<M> params(2 * Matrix::Ones(M, N));
-  ParameterMatrix<M> expected(Matrix::Ones(M, N));
-
-  // Subtract vector
-  EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
-  // Subtract another ParameterMatrix
-  ParameterMatrix<M> actual = params - ParameterMatrix<M>(Matrix::Ones(M, N));
-  EXPECT(assert_equal(expected, actual));
-}
-
-//******************************************************************************
-TEST(ParameterMatrix, Multiplication) {
-  ParameterMatrix<M> params(Matrix::Ones(M, N));
-  Matrix multiplier = 2 * Matrix::Ones(N, 2);
-  Matrix expected = 2 * N * Matrix::Ones(M, 2);
-  EXPECT(assert_equal(expected, params * multiplier));
-}
-
-//******************************************************************************
-TEST(ParameterMatrix, VectorSpace) {
-  ParameterMatrix<M> params(Matrix::Ones(M, N));
-  // vector
-  EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
-  // identity
-  EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
-                      ParameterMatrix<M>(Matrix::Zero(M, 0))));
-}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************

gtsam/inference/inference.i

@@ -109,6 +109,7 @@ class Ordering {
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
                       gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
+  static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,

gtsam/nonlinear/nonlinear.i

@@ -25,7 +25,6 @@ namespace gtsam {
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/NavState.h>
-#include <gtsam/basis/ParameterMatrix.h>
 
 #include <gtsam/nonlinear/GraphvizFormatting.h>
 class GraphvizFormatting : gtsam::DotWriter {

gtsam/nonlinear/values.i

@@ -25,7 +25,6 @@ namespace gtsam {
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/NavState.h>
-#include <gtsam/basis/ParameterMatrix.h>
 
 #include <gtsam/linear/VectorValues.h>
 
@@ -96,21 +95,6 @@ class Values {
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
-  void insert(size_t j, const gtsam::ParameterMatrix<1>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<2>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<3>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<4>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<5>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<6>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<7>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<8>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<9>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<10>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<11>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<12>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<13>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
-  void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   template <T = {gtsam::Point2, gtsam::Point3}>
   void insert(size_t j, const T& val);
@@ -144,21 +128,6 @@ class Values {
   void update(size_t j, Vector vector);
   void update(size_t j, Matrix matrix);
   void update(size_t j, double c);
-  void update(size_t j, const gtsam::ParameterMatrix<1>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<2>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<3>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<4>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<5>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<6>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<7>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<8>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<9>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<10>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<11>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<12>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<13>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<14>& X);
-  void update(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   void insert_or_assign(size_t j, const gtsam::Point2& point2);
   void insert_or_assign(size_t j, const gtsam::Point3& point3);
@@ -199,21 +168,6 @@ class Values {
   void insert_or_assign(size_t j, Vector vector);
   void insert_or_assign(size_t j, Matrix matrix);
   void insert_or_assign(size_t j, double c);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X);
-  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   template <T = {gtsam::Point2,
                  gtsam::Point3,
@@ -243,22 +197,7 @@ class Values {
                  gtsam::NavState,
                  Vector,
                  Matrix,
-                 double,
+                 double}>
-                 gtsam::ParameterMatrix<1>,
-                 gtsam::ParameterMatrix<2>,
-                 gtsam::ParameterMatrix<3>,
-                 gtsam::ParameterMatrix<4>,
-                 gtsam::ParameterMatrix<5>,
-                 gtsam::ParameterMatrix<6>,
-                 gtsam::ParameterMatrix<7>,
-                 gtsam::ParameterMatrix<8>,
-                 gtsam::ParameterMatrix<9>,
-                 gtsam::ParameterMatrix<10>,
-                 gtsam::ParameterMatrix<11>,
-                 gtsam::ParameterMatrix<12>,
-                 gtsam::ParameterMatrix<13>,
-                 gtsam::ParameterMatrix<14>,
-                 gtsam::ParameterMatrix<15>}>
   T at(size_t j);
 };