Chebyshev2 Improvements

gtsam/basis/Basis.h

@@ -239,8 +239,8 @@ class Basis {
      * i.e., one row of the Kronecker product of weights_ with the
      * MxM identity matrix. See also VectorEvaluationFunctor.
      */
-    void calculateJacobian(size_t N) {
-      H_.setZero(1, M_ * N);
+    void calculateJacobian() {
+      H_.setZero(1, M_ * EvaluationFunctor::weights_.size());
       for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
         H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j);
     }
@@ -252,14 +252,14 @@ class Basis {
     /// Construct with row index
     VectorComponentFunctor(size_t M, size_t N, size_t i, double x)
         : EvaluationFunctor(N, x), M_(M), rowIndex_(i) {
-      calculateJacobian(N);
+      calculateJacobian();
     }
 
     /// Construct with row index and interval
     VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a,
                            double b)
         : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) {
-      calculateJacobian(N);
+      calculateJacobian();
     }
 
     /// Calculate component of component rowIndex_ of P
@@ -460,8 +460,8 @@ class Basis {
      * i.e., one row of the Kronecker product of weights_ with the
      * MxM identity matrix. See also VectorDerivativeFunctor.
      */
-    void calculateJacobian(size_t N) {
-      H_.setZero(1, M_ * N);
+    void calculateJacobian() {
+      H_.setZero(1, M_ * this->weights_.size());
       for (int j = 0; j < this->weights_.size(); j++)
         H_(0, rowIndex_ + j * M_) = this->weights_(j);
     }
@@ -473,14 +473,14 @@ class Basis {
     /// Construct with row index
     ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x)
         : DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) {
-      calculateJacobian(N);
+      calculateJacobian();
     }
 
     /// Construct with row index and interval
     ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a,
                                double b)
         : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) {
-      calculateJacobian(N);
+      calculateJacobian();
     }
     /// Calculate derivative of component rowIndex_ of F
     double apply(const Matrix& P,

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).
    *

gtsam/basis/Chebyshev2.cpp

@@ -22,17 +22,17 @@ namespace gtsam {
 
 Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
   // Allocate space for weights
-  Weights weights(N);
+  Weights weights(N + 1);
 
   // We start by getting distances from x to all Chebyshev points
-  // as well as getting smallest distance
-  Weights distances(N);
+  // as well as getting the smallest distance
+  Weights distances(N + 1);
 
-  for (size_t j = 0; j < N; j++) {
+  for (size_t j = 0; j <= N; j++) {
     const double dj =
         x - Point(N, j, a, b);  // only thing that depends on [a,b]
 
-    if (std::abs(dj) < 1e-10) {
+    if (std::abs(dj) < 1e-12) {
       // exceptional case: x coincides with a Chebyshev point
       weights.setZero();
       weights(j) = 1;
@@ -44,55 +44,64 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
   // Beginning of interval, j = 0, x(0) = a
   weights(0) = 0.5 / distances(0);
 
-  // All intermediate points j=1:N-2
+  // All intermediate points j=1:N-1
   double d = weights(0), s = -1;  // changes sign s at every iteration
-  for (size_t j = 1; j < N - 1; j++, s = -s) {
+  for (size_t j = 1; j < N; j++, s = -s) {
     weights(j) = s / distances(j);
     d += weights(j);
   }
 
-  // End of interval, j = N-1, x(N-1) = b
-  weights(N - 1) = 0.5 * s / distances(N - 1);
-  d += weights(N - 1);
+  // End of interval, j = N, x(N) = b
+  weights(N) = 0.5 * s / distances(N);
+  d += weights(N);
 
   // normalize
   return weights / d;
 }
 
+Matrix Chebyshev2::WeightMatrix(size_t N, const Vector& X, double a, double b) {
+  // Chebyshev points go from 0 to N, hence N+1 points.
+  Matrix W(X.size(), N + 1);
+  for (int i = 0; i < X.size(); i++) {
+    W.row(i) = CalculateWeights(N, X(i), a, b);
+  }
+  return W;
+}
+
 Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
   // Allocate space for weights
-  Weights weightDerivatives(N);
+  Weights weightDerivatives(N + 1);
 
   // toggle variable so we don't need to use `pow` for -1
   double t = -1;
 
   // We start by getting distances from x to all Chebyshev points
   // as well as getting smallest distance
-  Weights distances(N);
+  Weights distances(N + 1);
 
-  for (size_t j = 0; j < N; j++) {
+  for (size_t j = 0; j <= N; j++) {
     const double dj =
         x - Point(N, j, a, b);  // only thing that depends on [a,b]
-    if (std::abs(dj) < 1e-10) {
+    if (std::abs(dj) < 1e-12) {
       // exceptional case: x coincides with a Chebyshev point
       weightDerivatives.setZero();
       // compute the jth row of the differentiation matrix for this point
-      double cj = (j == 0 || j == N - 1) ? 2. : 1.;
-      for (size_t k = 0; k < N; k++) {
+      double cj = (j == 0 || j == N) ? 2. : 1.;
+      for (size_t k = 0; k <= N; k++) {
         if (j == 0 && k == 0) {
           // we reverse the sign since we order the cheb points from -1 to 1
-          weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0;
-        } else if (j == N - 1 && k == N - 1) {
+          weightDerivatives(k) = -(cj * N * N + 1) / 6.0;
+        } else if (j == N && k == N) {
           // we reverse the sign since we order the cheb points from -1 to 1
-          weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0;
+          weightDerivatives(k) = (cj * N * N + 1) / 6.0;
         } else if (k == j) {
           double xj = Point(N, j);
           double xj2 = xj * xj;
           weightDerivatives(k) = -0.5 * xj / (1 - xj2);
         } else {
           double xj = Point(N, j);
           double xk = Point(N, k);
-          double ck = (k == 0 || k == N - 1) ? 2. : 1.;
+          double ck = (k == 0 || k == N) ? 2. : 1.;
           t = ((j + k) % 2) == 0 ? 1 : -1;
           weightDerivatives(k) = (cj / ck) * t / (xj - xk);
         }
@@ -111,8 +120,8 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
   double g = 0, k = 0;
   double w = 1;
 
-  for (size_t j = 0; j < N; j++) {
-    if (j == 0 || j == N - 1) {
+  for (size_t j = 0; j <= N; j++) {
+    if (j == 0 || j == N) {
       w = 0.5;
     } else {
       w = 1.0;
@@ -128,13 +137,13 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
   double s = 1;  // changes sign s at every iteration
   double g2 = g * g;
 
-  for (size_t j = 0; j < N; j++, s = -s) {
-    // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1,
-    // x0 = 1.0
-    if (j == 0 || j == N - 1) {
+  for (size_t j = 0; j <= N; j++, s = -s) {
+    // Beginning of interval, j = 0, x0 = -1.0
+    // and end of interval, j = N, x0 = 1.0
+    if (j == 0 || j == N) {
       w = 0.5;
     } else {
-      // All intermediate points j=1:N-2
+      // All intermediate points j=1:N-1
       w = 1.0;
     }
     weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) -
@@ -146,31 +155,31 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
 
 Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
                                                          double b) {
-  DiffMatrix D(N, N);
-  if (N == 1) {
+  DiffMatrix D(N + 1, N + 1);
+  if (N + 1 == 1) {
     D(0, 0) = 1;
     return D;
   }
 
   // toggle variable so we don't need to use `pow` for -1
   double t = -1;
 
-  for (size_t i = 0; i < N; i++) {
+  for (size_t i = 0; i <= N; i++) {
     double xi = Point(N, i);
-    double ci = (i == 0 || i == N - 1) ? 2. : 1.;
-    for (size_t j = 0; j < N; j++) {
+    double ci = (i == 0 || i == N) ? 2. : 1.;
+    for (size_t j = 0; j <= N; j++) {
       if (i == 0 && j == 0) {
         // we reverse the sign since we order the cheb points from -1 to 1
-        D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0;
-      } else if (i == N - 1 && j == N - 1) {
+        D(i, j) = -(ci * N * N + 1) / 6.0;
+      } else if (i == N && j == N) {
         // we reverse the sign since we order the cheb points from -1 to 1
-        D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0;
+        D(i, j) = (ci * N * N + 1) / 6.0;
       } else if (i == j) {
         double xi2 = xi * xi;
         D(i, j) = -xi / (2 * (1 - xi2));
       } else {
         double xj = Point(N, j);
-        double cj = (j == 0 || j == N - 1) ? 2. : 1.;
+        double cj = (j == 0 || j == N) ? 2. : 1.;
         t = ((i + j) % 2) == 0 ? 1 : -1;
         D(i, j) = (ci / cj) * t / (xi - xj);
       }
@@ -182,15 +191,15 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
 
 Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
   // Allocate space for weights
-  Weights weights(N);
-  size_t K = N - 1,  // number of intervals between N points
+  Weights weights(N + 1);
+  size_t K = N,  // number of intervals between N points
       K2 = K * K;
   weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
-  weights(N - 1) = weights(0);
+  weights(N) = weights(0);
 
   size_t last_k = K / 2 + K % 2 - 1;
 
-  for (size_t i = 1; i <= N - 2; ++i) {
+  for (size_t i = 1; i <= N - 1; ++i) {
     double theta = i * M_PI / K;
     weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;
 

gtsam/basis/Chebyshev2.h

@@ -51,27 +51,30 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
   using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
 
-  /// Specific Chebyshev point
-  static double Point(size_t N, int j) {
-    assert(j >= 0 && size_t(j) < N);
-    const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
-    // We add -PI so that we get values ordered from -1 to +1
-    // sin(- M_PI_2 + dtheta*j); also works
-    return cos(-M_PI + dtheta * j);
-  }
-
-  /// Specific Chebyshev point, within [a,b] interval
-  static double Point(size_t N, int j, double a, double b) {
-    assert(j >= 0 && size_t(j) < N);
-    const double dtheta = M_PI / (N - 1);
+  /**
+   * @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)
+   * @param b Upper bound of interval (default: 1)
+   * @return double
+   */
+  static double Point(size_t N, int j, double a = -1, double b = 1) {
+    assert(j >= 0 && size_t(j) <= N);
+    const double dtheta = M_PI / (N > 1 ? N : 1);
     // We add -PI so that we get values ordered from -1 to +1
+    // sin(-M_PI_2 + dtheta*j); also works
     return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
   }
 
   /// All Chebyshev points
   static Vector Points(size_t N) {
-    Vector points(N);
-    for (size_t j = 0; j < N; j++) points(j) = Point(N, j);
+    Vector points(N + 1);
+    for (size_t j = 0; j <= N; j++) {
+      points(j) = Point(N, j);
+    }
     return points;
   }
 
@@ -83,8 +86,13 @@ 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)
+   * Evaluate Chebyshev Weights on [-1,1] at any x up to order N (N+1 values)
    * These weights implement barycentric interpolation at a specific x.
    * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the
    * values of the function f at the Chebyshev points. As such, for a given x we
@@ -94,6 +102,16 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   static Weights CalculateWeights(size_t N, double x, double a = -1,
                                   double b = 1);
 
+  /**
+   * Calculate weights for all x in vector X.
+   * Returns M*N matrix where M is the size of the vector X,
+   * and N is the number of basis functions.
+   *
+   * Overriden for Chebyshev2.
+   */
+  static Matrix WeightMatrix(size_t N, const Vector& X, double a = -1,
+                             double b = 1);
+
   /**
    *  Evaluate derivative of barycentric weights.
    *  This is easy and efficient via the DifferentiationMatrix.
@@ -134,8 +152,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   template <size_t M>
   static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
                        size_t N, double a = -1, double b = 1) {
-    Matrix Xmat(M, N);
-    for (size_t j = 0; j < N; j++) {
+    Matrix Xmat(M, N + 1);
+    for (size_t j = 0; j <= N; j++) {
       Xmat.col(j) = f(Point(N, j, a, b));
     }
     return Xmat;

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;
   }

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)