Merge pull request #2069 from borglab/feature/faster_chebs

Faster Chebyshev2

gtsam/basis/Chebyshev2.cpp

@@ -17,37 +17,117 @@
  */
 
 #include <gtsam/basis/Chebyshev2.h>
+
+#include <Eigen/Dense>
+
 #include <cassert>
 
 namespace gtsam {
 
-double Chebyshev2::Point(size_t N, int j, double a, double b) {
+double Chebyshev2::Point(size_t N, int j) {
+  if (N == 1) return 0.0;
   assert(j >= 0 && size_t(j) < N);
-  const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
+  const double dTheta = M_PI / (N - 1);
-  // We add -PI so that we get values ordered from -1 to +1
+  return -cos(dTheta * j);
-  // sin(-M_PI_2 + dtheta*j); also works
-  return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
 }
 
-Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
+double Chebyshev2::Point(size_t N, int j, double a, double b) {
-  // Allocate space for weights
+  if (N == 1) return (a + b) / 2;
-  Weights weights(N);
+  return a + (b - a) * (Point(N, j) + 1.0) / 2.0;
+}
 
-  // We start by getting distances from x to all Chebyshev points
+Vector Chebyshev2::Points(size_t N) {
-  // as well as getting smallest distance
+  Vector points(N);
-  Weights distances(N);
+  if (N == 1) {
+    points(0) = 0.0;
+    return points;
+  }
+  size_t half = N / 2;
+  const double dTheta = M_PI / (N - 1);
+  for (size_t j = 0; j < half; ++j) {
+    double c = cos(j * dTheta);
+    points(j) = -c;
+    points(N - 1 - j) = c;
+  }
+  if (N % 2 == 1) {
+    points(half) = 0.0;
+  }
+  return points;
+}
 
-  for (size_t j = 0; j < N; j++) {
+Vector Chebyshev2::Points(size_t N, double a, double b) {
-    const double dj =
+  Vector points = Points(N);
-        x - Point(N, j, a, b);  // only thing that depends on [a,b]
+  const double T1 = (a + b) / 2, T2 = (b - a) / 2;
+  points = T1 + (T2 * points).array();
+  return points;
+}
 
-    if (std::abs(dj) < 1e-12) {
+namespace {
-      // exceptional case: x coincides with a Chebyshev point
+  // Find the index of the Chebyshev point that coincides with x
-      weights.setZero();
+  // within the interval [a, b]. If no such point exists, return nullopt.
-      weights(j) = 1;
+  std::optional<size_t> coincidentPoint(size_t N, double x, double a, double b, double tol = 1e-12) {
-      return weights;
+    if (N == 0) return std::nullopt;
+    if (N == 1) {
+      double mid = (a + b) / 2;
+      if (std::abs(x - mid) < tol) return 0;
+    } else {
+      // Compute normalized value y such that cos(j*dTheta) = y.
+      double y = 1.0 - 2.0 * (x - a) / (b - a);
+      if (y < -1.0 || y > 1.0) return std::nullopt;
+      double dTheta = M_PI / (N - 1);
+      double jCandidate = std::acos(y) / dTheta;
+      size_t jRounded = static_cast<size_t>(std::round(jCandidate));
+      if (std::abs(jCandidate - jRounded) < tol) return jRounded;
     }
-    distances(j) = dj;
+    return std::nullopt;
+  }
+
+  // Get signed distances from x to all Chebyshev points
+  Vector signedDistances(size_t N, double x, double a, double b) {
+    Vector result(N);
+    const Vector points = Chebyshev2::Points(N, a, b);
+    for (size_t j = 0; j < N; j++) {
+      const double dj = x - points[j];
+      result(j) = dj;
+    }
+    return result;
+  }
+
+  // Helper function to calculate a row of the differentiation matrix, [-1,1] interval
+  Vector differentiationMatrixRow(size_t N, const Vector& points, size_t i) {
+    Vector row(N);
+    const size_t K = N - 1;
+    double xi = points(i);
+    for (size_t j = 0; j < N; j++) {
+      if (i == j) {
+        // Diagonal elements
+        if (i == 0 || i == K)
+          row(j) = (i == 0 ? -1 : 1) * (2.0 * K * K + 1) / 6.0;
+        else
+          row(j) = -xi / (2.0 * (1.0 - xi * xi));
+      }
+      else {
+        double xj = points(j);
+        double ci = (i == 0 || i == K) ? 2. : 1.;
+        double cj = (j == 0 || j == K) ? 2. : 1.;
+        double t = ((i + j) % 2) == 0 ? 1 : -1;
+        row(j) = (ci / cj) * t / (xi - xj);
+      }
+    }
+    return row;
+  }
+}  // namespace
+
+Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
+  // We start by getting distances from x to all Chebyshev points
+  const Vector distances = signedDistances(N, x, a, b);
+
+  Weights weights(N);
+  if (auto j = coincidentPoint(N, x, a, b)) {
+    // exceptional case: x coincides with a Chebyshev point
+    weights.setZero();
+    weights(*j) = 1;
+    return weights;
   }
 
   // Beginning of interval, j = 0, x(0) = a
@@ -69,75 +149,40 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
 }
 
 Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
-  // Allocate space for weights
+  if (auto j = coincidentPoint(N, x, a, b)) {
-  Weights weightDerivatives(N);
+    // exceptional case: x coincides with a Chebyshev point
-
+    return differentiationMatrixRow(N, Points(N), *j) / ((b - a) / 2.0);
-  // 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);
-
-  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-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++) {
-        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) {
-          // 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 (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.;
-          t = ((j + k) % 2) == 0 ? 1 : -1;
-          weightDerivatives(k) = (cj / ck) * t / (xj - xk);
-        }
-      }
-      return 2 * weightDerivatives / (b - a);
-    }
-    distances(j) = dj;
   }
-
+  
   // This section of code computes the derivative of
   // the Barycentric Interpolation weights formula by applying
   // the chain rule on the original formula.
-
+  
   // g and k are multiplier terms which represent the derivatives of
   // the numerator and denominator
   double g = 0, k = 0;
-  double w = 1;
+  double w;
-
+  
+  const Vector distances = signedDistances(N, x, a, b);
   for (size_t j = 0; j < N; j++) {
     if (j == 0 || j == N - 1) {
       w = 0.5;
     } else {
       w = 1.0;
     }
-
+    
-    t = (j % 2 == 0) ? 1 : -1;
+    double t = (j % 2 == 0) ? 1 : -1;
-
+    
     double c = t / distances(j);
     g += w * c;
     k += (w * c / distances(j));
   }
-
+  
   double s = 1;  // changes sign s at every iteration
   double g2 = g * g;
-
+  
-  for (size_t j = 0; j < N; j++, s = -s) {
+  Weights weightDerivatives(N);
+  for (size_t j = 0; j < N; j++) {
     // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1,
     // x0 = 1.0
     if (j == 0 || j == N - 1) {
@@ -148,67 +193,121 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
     }
     weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) -
                            (w * -s * k / (g2 * distances(j)));
+    s *= -1;
   }
 
   return weightDerivatives;
 }
 
-Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
+Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N) {
-                                                         double b) {
   DiffMatrix D(N, N);
   if (N == 1) {
     D(0, 0) = 1;
     return D;
   }
 
-  // toggle variable so we don't need to use `pow` for -1
+  const Vector points = Points(N);
-  double t = -1;
-
   for (size_t i = 0; i < N; i++) {
-    double xi = Point(N, i);
+    D.row(i) = differentiationMatrixRow(N, points, i);
-    double ci = (i == 0 || i == N - 1) ? 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) {
-        // 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 == 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.;
-        t = ((i + j) % 2) == 0 ? 1 : -1;
-        D(i, j) = (ci / cj) * t / (xi - xj);
-      }
-    }
   }
-  // scale the matrix to the range
+  return D;
-  return D / ((b - a) / 2.0);
 }
 
-Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
+Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, double b) {
-  // Allocate space for weights
+  DiffMatrix D(N, N);
-  Weights weights(N);
+  if (N == 1) {
-  size_t K = N - 1,  // number of intervals between N points
+    D(0, 0) = 1;
-      K2 = K * K;
+    return D;
-  weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
-  weights(N - 1) = weights(0);
-
-  size_t last_k = K / 2 + K % 2 - 1;
-
-  for (size_t i = 1; i <= N - 2; ++i) {
-    double theta = i * M_PI / K;
-    weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;
-
-    for (size_t k = 1; k <= last_k; ++k)
-      weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1);
-    weights(i) *= (b - a) / K;
   }
 
+  // Calculate for [-1,1] and scale for [a,b]
+  return DifferentiationMatrix(N) / ((b - a) / 2.0);
+}
+
+Matrix Chebyshev2::IntegrationMatrix(size_t N) {
+  // Obtain the differentiation matrix.
+  const Matrix D = DifferentiationMatrix(N);
+
+  // Compute the pseudo-inverse of the differentiation matrix.
+  Eigen::JacobiSVD<Matrix> svd(D, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  const auto& S = svd.singularValues();
+  Matrix invS = Matrix::Zero(N, N);
+  for (size_t i = 0; i < N - 1; ++i) invS(i, i) = 1.0 / S(i);
+  Matrix P = svd.matrixV() * invS * svd.matrixU().transpose();
+
+  // Return a version of P that makes sure (P*f)(0) = 0.
+  const Weights row0 = P.row(0);
+  P.rowwise() -= row0;
+  return P;
+}
+
+Matrix Chebyshev2::IntegrationMatrix(size_t N, double a, double b) {
+  return IntegrationMatrix(N) * (b - a) / 2.0;
+}
+
+/*
+   Trefethen00book, pg 128, clencurt.m
+   Note that N in clencurt.m is 1 less than our N, we call it K below.
+   K = N-1;
+   theta = pi*(0:K)'/K;
+   w = zeros(1,N); ii = 2:K; v = ones(K-1, 1);
+   if mod(K,2) == 0
+       w(1) = 1/(K^2-1); w(N) = w(1);
+       for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
+       v = v - cos(K*theta(ii))/(K^2-1);
+   else
+       w(1) = 1/K^2; w(N) = w(1);
+       for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
+   end
+   w(ii) = 2*v/K;
+*/
+Weights Chebyshev2::IntegrationWeights(size_t N) {
+  Weights weights(N);
+  const size_t K = N - 1,  // number of intervals between N points
+    K2 = K * K;
+
+  // Compute endpoint weight.
+  weights(0) = 1.0 / (K2 + K % 2 - 1);
+  weights(N - 1) = weights(0);
+
+  // Compute up to the middle; mirror symmetry holds.
+  const size_t mid = (N - 1) / 2;
+  double dTheta = M_PI / K;
+  for (size_t i = 1; i <= mid; ++i) {
+    double w = (K % 2 == 0) ? 1 - cos(i * M_PI) / (K2 - 1) : 1;
+    const size_t last_k = K / 2 + K % 2 - 1;
+    const double theta = i * dTheta;
+    for (size_t k = 1; k <= last_k; ++k)
+      w -= 2.0 * cos(2 * k * theta) / (4 * k * k - 1);
+    w *= 2.0 / K;
+    weights(i) = w;
+    weights(N - 1 - i) = w;
+  }
   return weights;
 }
 
+Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
+  return IntegrationWeights(N) * (b - a) / 2.0;
+}
+
+Weights Chebyshev2::DoubleIntegrationWeights(size_t N) {
+  // we have w * P, where w are the Clenshaw-Curtis weights and P is the integration matrix
+  // But P does not by default return a function starting at zero.
+  return Chebyshev2::IntegrationWeights(N) * Chebyshev2::IntegrationMatrix(N);
+}
+
+Weights Chebyshev2::DoubleIntegrationWeights(size_t N, double a, double b) {
+  return Chebyshev2::IntegrationWeights(N, a, b) * Chebyshev2::IntegrationMatrix(N, a, b);
+}
+
+/**
+ * Create vector of values at Chebyshev points given scalar-valued function.
+ */
+Vector Chebyshev2::vector(std::function<double(double)> f, size_t N, double a, double b) {
+  Vector fvals(N);
+  const Vector points = Points(N, a, b);
+  for (size_t j = 0; j < N; j++) fvals(j) = f(points(j));
+  return fvals;
+}
+
 }  // namespace gtsam

gtsam/basis/Chebyshev2.h

@@ -51,9 +51,17 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
   using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
 
+  /**
+   * @brief Specific Chebyshev point, within [-1,1] interval.
+   *
+   * @param N The degree of the polynomial
+   * @param j The index of the Chebyshev point
+   * @return double
+   */
+  static double Point(size_t N, int j);
+
   /**
    * @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
@@ -61,24 +69,13 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
    * @param b Upper bound of interval (default: 1)
    * @return double
    */
-  static double Point(size_t N, int j, double a = -1, double b = 1);
+  static double Point(size_t N, int j, double a, double b);
 
   /// All Chebyshev points
-  static Vector Points(size_t N) {
+  static Vector Points(size_t N);
-    Vector points(N);
-    for (size_t j = 0; j < N; j++) {
-      points(j) = Point(N, j);
-    }
-    return points;
-  }
 
   /// All Chebyshev points, within [a,b] interval
-  static Vector Points(size_t N, double a, double b) {
+  static Vector Points(size_t N, double a, double b);
-    Vector points = Points(N);
-    const double T1 = (a + b) / 2, T2 = (b - a) / 2;
-    points = T1 + (T2 * points).array();
-    return points;
-  }
 
   /**
    * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values)
@@ -88,53 +85,61 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
    * obtain a linear map from parameter vectors f to interpolated values f(x).
    * Optional [a,b] interval can be specified as well.
    */
-  static Weights CalculateWeights(size_t N, double x, double a = -1,
+  static Weights CalculateWeights(size_t N, double x, double a = -1, double b = 1);
-                                  double b = 1);
 
   /**
    *  Evaluate derivative of barycentric weights.
    *  This is easy and efficient via the DifferentiationMatrix.
    */
-  static Weights DerivativeWeights(size_t N, double x, double a = -1,
+  static Weights DerivativeWeights(size_t N, double x, double a = -1, double b = 1);
-                                   double b = 1);
 
-  /// compute D = differentiation matrix, Trefethen00book p.53
+  /// Compute D = differentiation matrix, Trefethen00book p.53
-  /// when given a parameter vector f of function values at the Chebyshev
+  /// When given a parameter vector f of function values at the Chebyshev
   /// points, D*f are the values of f'.
   /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4
-  static DiffMatrix DifferentiationMatrix(size_t N, double a = -1,
+  static DiffMatrix DifferentiationMatrix(size_t N);
-                                          double b = 1);
+
+  /// Compute D = differentiation matrix, for interval [a,b]
+  static DiffMatrix DifferentiationMatrix(size_t N, double a, double b);
+
+  /// IntegrationMatrix returns the (N+1)×(N+1) matrix P such that for any f,
+  /// F = P * f recovers F (the antiderivative) satisfying f = D * F and F(0)=0.
+  static Matrix IntegrationMatrix(size_t N);
+
+  /// IntegrationMatrix returns the (N+1)×(N+1) matrix P for interval [a,b]
+  static Matrix IntegrationMatrix(size_t N, double a, double b);
 
   /**
-   *  Evaluate Clenshaw-Curtis integration weights.
+   *  Calculate Clenshaw-Curtis integration weights.
    *  Trefethen00book, pg 128, clencurt.m
    *  Note that N in clencurt.m is 1 less than our N
-   *  K = N-1;
-      theta = pi*(0:K)'/K;
-      w = zeros(1,N); ii = 2:K; v = ones(K-1, 1);
-      if mod(K,2) == 0
-          w(1) = 1/(K^2-1); w(N) = w(1);
-          for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
-          v = v - cos(K*theta(ii))/(K^2-1);
-      else
-          w(1) = 1/K^2; w(N) = w(1);
-          for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
-      end
-      w(ii) = 2*v/K;
-
    */
-  static Weights IntegrationWeights(size_t N, double a = -1, double b = 1);
+  static Weights IntegrationWeights(size_t N);
+
+  /// Calculate Clenshaw-Curtis integration weights, for interval [a,b]
+  static Weights IntegrationWeights(size_t N, double a, double b);
 
   /**
-   * Create matrix of values at Chebyshev points given vector-valued function.
+   * Calculate Double Clenshaw-Curtis integration weights
+   * We compute them as W * P, where W are the Clenshaw-Curtis weights and P is
+   * the integration matrix.
    */
+  static Weights DoubleIntegrationWeights(size_t N);
+
+  /// Calculate Double Clenshaw-Curtis integration weights, for interval [a,b]
+  static Weights DoubleIntegrationWeights(size_t N, double a, double b);
+
+  /// Create matrix of values at Chebyshev points given vector-valued function.
+  static Vector vector(std::function<double(double)> f,
+    size_t N, double a = -1, double b = 1);
+
+  /// Create matrix of values at Chebyshev points given vector-valued function.
   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) {
+    size_t N, double a = -1, double b = 1) {
     Matrix Xmat(M, N);
-    for (size_t j = 0; j < N; j++) {
+    const Vector points = Points(N, a, b);
-      Xmat.col(j) = f(Point(N, j, a, b));
+    for (size_t j = 0; j < N; j++) Xmat.col(j) = f(points(j));
-    }
     return Xmat;
   }
 };  // \ Chebyshev2

gtsam/basis/tests/testChebyshev2.cpp

@@ -9,13 +9,13 @@
 
  * -------------------------------------------------------------------------- */
 
-/**
+ /**
- * @file testChebyshev2.cpp
+  * @file testChebyshev2.cpp
- * @date July 4, 2020
+  * @date July 4, 2020
- * @author Varun Agrawal
+  * @author Varun Agrawal
- * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral
+  * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral
- *        methods
+  *        methods
- */
+  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
@@ -28,18 +28,11 @@
 #include <cstddef>
 #include <functional>
 
-using namespace std;
 using namespace gtsam;
 
-namespace {
-noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
-
-const size_t N = 32;
-}  // namespace
-
 //******************************************************************************
 TEST(Chebyshev2, Point) {
-  static const int N = 5;
+  static const size_t N = 5;
   auto points = Chebyshev2::Points(N);
   Vector expected(N);
   expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.;
@@ -57,7 +50,7 @@ TEST(Chebyshev2, Point) {
 
 //******************************************************************************
 TEST(Chebyshev2, PointInInterval) {
-  static const int N = 5;
+  static const size_t N = 5;
   auto points = Chebyshev2::Points(N, 0, 20);
   Vector expected(N);
   expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.;
@@ -77,7 +70,7 @@ TEST(Chebyshev2, PointInInterval) {
 //******************************************************************************
 // InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5]
 TEST(Chebyshev2, Interpolate2) {
-  size_t N = 3;
+  const size_t N = 3;
   Chebyshev2::EvaluationFunctor fx(N, 0.5);
   Vector f(N);
   f << 4, 2, 6;
@@ -121,16 +114,17 @@ TEST(Chebyshev2, InterpolateVector) {
 
   // Check derivative
   std::function<Vector2(Matrix)> f =
-      std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
+    std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
-                std::placeholders::_1, nullptr);
+      std::placeholders::_1, nullptr);
   Matrix numericalH =
-      numericalDerivative11<Vector2, Matrix, 2 * N>(f, X);
+    numericalDerivative11<Vector2, Matrix, 2 * N>(f, X);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
 //******************************************************************************
 // Interpolating poses using the exponential map
 TEST(Chebyshev2, InterpolatePose2) {
+  const size_t N = 32;
   double t = 30, a = 0, b = 100;
 
   Matrix X(3, N);
@@ -149,10 +143,10 @@ TEST(Chebyshev2, InterpolatePose2) {
 
   // Check derivative
   std::function<Pose2(Matrix)> f =
-      std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
+    std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
-                std::placeholders::_1, nullptr);
+      std::placeholders::_1, nullptr);
   Matrix numericalH =
-      numericalDerivative11<Pose2, Matrix, 3 * N>(f, X);
+    numericalDerivative11<Pose2, Matrix, 3 * N>(f, X);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
@@ -160,6 +154,7 @@ TEST(Chebyshev2, InterpolatePose2) {
 //******************************************************************************
 // Interpolating poses using the exponential map
 TEST(Chebyshev2, InterpolatePose3) {
+  const size_t N = 32;
   double a = 10, b = 100;
   double t = Chebyshev2::Points(N, a, b)(11);
 
@@ -179,10 +174,10 @@ TEST(Chebyshev2, InterpolatePose3) {
 
   // Check derivative
   std::function<Pose3(Matrix)> f =
-      std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
+    std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
-                std::placeholders::_1, nullptr);
+      std::placeholders::_1, nullptr);
   Matrix numericalH =
-      numericalDerivative11<Pose3, Matrix, 6 * N>(f, X);
+    numericalDerivative11<Pose3, Matrix, 6 * N>(f, X);
   EXPECT(assert_equal(numericalH, actualH, 1e-8));
 }
 #endif
@@ -197,7 +192,7 @@ TEST(Chebyshev2, Decomposition) {
   }
 
   // Do Chebyshev Decomposition
-  FitBasis<Chebyshev2> actual(sequence, model, 3);
+  FitBasis<Chebyshev2> actual(sequence, nullptr, 3);
 
   // Check
   Vector expected(3);
@@ -212,8 +207,8 @@ TEST(Chebyshev2, DifferentiationMatrix3) {
   Matrix expected(N, N);
   // Differentiation matrix computed from chebfun
   expected << 1.5000, -2.0000, 0.5000,  //
-      0.5000, -0.0000, -0.5000,         //
+    0.5000, -0.0000, -0.5000,         //
-      -0.5000, 2.0000, -1.5000;
+    -0.5000, 2.0000, -1.5000;
   // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen
   // This was verified with chebfun
   expected = -expected;
@@ -228,11 +223,11 @@ TEST(Chebyshev2, DerivativeMatrix6) {
   const size_t N = 6;
   Matrix expected(N, N);
   expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000,  //
-      2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764,           //
+    2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764,           //
-      -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820,          //
+    -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820,          //
-      0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236,            //
+    0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236,            //
-      -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180,           //
+    -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180,           //
-      0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000;
+    0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000;
   // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen
   // This was verified with chebfun
   expected = -expected;
@@ -255,10 +250,8 @@ double fprime(double x) {
 
 //******************************************************************************
 TEST(Chebyshev2, CalculateWeights) {
-  Eigen::Matrix<double, -1, 1> fvals(N);
+  const size_t N = 32;
-  for (size_t i = 0; i < N; i++) {
+  Vector fvals = Chebyshev2::vector(f, N);
-    fvals(i) = f(Chebyshev2::Point(N, i));
-  }
   double x1 = 0.7, x2 = -0.376;
   Weights weights1 = Chebyshev2::CalculateWeights(N, x1);
   Weights weights2 = Chebyshev2::CalculateWeights(N, x2);
@@ -267,12 +260,9 @@ TEST(Chebyshev2, CalculateWeights) {
 }
 
 TEST(Chebyshev2, CalculateWeights2) {
+  const size_t N = 32;
   double a = 0, b = 10, x1 = 7, x2 = 4.12;
-
+  Vector fvals = Chebyshev2::vector(f, N, a, b);
-  Eigen::Matrix<double, -1, 1> fvals(N);
-  for (size_t i = 0; i < N; i++) {
-    fvals(i) = f(Chebyshev2::Point(N, i, a, b));
-  }
 
   Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b);
   EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8);
@@ -283,34 +273,39 @@ TEST(Chebyshev2, CalculateWeights2) {
   EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8);
 }
 
-TEST(Chebyshev2, DerivativeWeights) {
+// Test CalculateWeights when a point coincides with a Chebyshev point
-  Eigen::Matrix<double, -1, 1> fvals(N);
+TEST(Chebyshev2, CalculateWeights_CoincidingPoint) {
-  for (size_t i = 0; i < N; i++) {
+  const size_t N = 5;
-    fvals(i) = f(Chebyshev2::Point(N, i));
+  const double coincidingPoint = Chebyshev2::Point(N, 1);  // Pick the 2nd point
+
+  // Generate weights for the coinciding point
+  Weights weights = Chebyshev2::CalculateWeights(N, coincidingPoint);
+
+  // Verify that the weights are zero everywhere except at the coinciding point
+  for (size_t j = 0; j < N; ++j) {
+    EXPECT_DOUBLES_EQUAL(j == 1 ? 1.0 : 0.0, weights(j), 1e-9);
   }
-  double x1 = 0.7, x2 = -0.376, x3 = 0.0;
+}
-  Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1);
-  EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9);
 
-  Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2);
+TEST(Chebyshev2, DerivativeWeights) {
-  EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9);
+  const size_t N = 32;
+  Vector fvals = Chebyshev2::vector(f, N);
+  std::vector<double> testPoints = { 0.7, -0.376, 0.0 };
+  for (double x : testPoints) {
+    Weights dWeights = Chebyshev2::DerivativeWeights(N, x);
+    EXPECT_DOUBLES_EQUAL(fprime(x), dWeights * fvals, 1e-9);
+  }
 
-  Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3);
+  // test if derivative calculation at Chebyshev point is correct
-  EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9);
-
-  // test if derivative calculation and cheb point is correct
   double x4 = Chebyshev2::Point(N, 3);
   Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4);
   EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9);
 }
 
 TEST(Chebyshev2, DerivativeWeights2) {
+  const size_t N = 32;
   double x1 = 5, x2 = 4.12, a = 0, b = 10;
-
+  Vector fvals = Chebyshev2::vector(f, N, a, b);
-  Eigen::Matrix<double, -1, 1> fvals(N);
-  for (size_t i = 0; i < N; i++) {
-    fvals(i) = f(Chebyshev2::Point(N, i, a, b));
-  }
 
   Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b);
   EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8);
@@ -318,12 +313,13 @@ TEST(Chebyshev2, DerivativeWeights2) {
   Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b);
   EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8);
 
-  // test if derivative calculation and Chebyshev point is correct
+  // test if derivative calculation at Chebyshev point is correct
   double x3 = Chebyshev2::Point(N, 3, a, b);
   Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b);
   EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8);
 }
 
+
 //******************************************************************************
 // Check two different ways to calculate the derivative weights
 TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) {
@@ -366,9 +362,8 @@ double proxy3(double x) {
   return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points);
 }
 
+// Check Derivative evaluation at point x=0.2
 TEST(Chebyshev2, Derivative6) {
-  // Check Derivative evaluation at point x=0.2
-
   // calculate expected values by numerical derivative of synthesis
   const double x = 0.2;
   Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy3, x);
@@ -420,15 +415,15 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
   EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
 
   // Test Jacobian
-  Matrix expectedH = numericalDerivative11<Vector2, Matrix, M * N>(
+  Matrix expectedH = numericalDerivative11<Vector2, Matrix, M* N>(
-      std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
+    std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
   EXPECT(assert_equal(expectedH, actualH, 1e-7));
 }
 
 //******************************************************************************
 // Test VectorDerivativeFunctor with polynomial function
 TEST(Chebyshev2, VectorDerivativeFunctor2) {
-  const size_t N = 64, M = 1, T = 15;
+  const size_t N = 4, M = 1, T = 15;
   using VecD = Chebyshev2::VectorDerivativeFunctor;
 
   const Vector points = Chebyshev2::Points(N, 0, T);
@@ -451,8 +446,8 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
   Matrix actualH(M, M * N);
   VecD vecd(M, N, points(0), 0, T);
   vecd(X, actualH);
-  Matrix expectedH = numericalDerivative11<Vector1, Matrix, M * N>(
+  Matrix expectedH = numericalDerivative11<Vector1, Matrix, M* N>(
-      std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
+    std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
   EXPECT(assert_equal(expectedH, actualH, 1e-6));
 }
 
@@ -468,28 +463,124 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
   Matrix actualH(1, M * N);
   EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
 
-  Matrix expectedH = numericalDerivative11<double, Matrix, M * N>(
+  Matrix expectedH = numericalDerivative11<double, Matrix, M* N>(
-      std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
+    std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
   EXPECT(assert_equal(expectedH, actualH, 1e-7));
 }
 
 //******************************************************************************
-TEST(Chebyshev2, IntegralWeights) {
+TEST(Chebyshev2, IntegrationMatrix) {
-  const size_t N7 = 7;
+  const size_t N = 10;  // number of intervals => N+1 nodes
-  Vector actual = Chebyshev2::IntegrationWeights(N7);
+  const double a = 0, b = 10;
-  Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254,
+
-                     0.457142857142857, 0.520634920634921, 0.457142857142857,
+  // Create integration matrix
-                     0.253968253968254, 0.0285714285714286)
+  Matrix P = Chebyshev2::IntegrationMatrix(N, a, b);
-                        .finished();
+
+  // Let's check that integrating a constant yields
+  // the sum of the lengths of the intervals:
+  Vector F = P * Vector::Ones(N);
+  EXPECT_DOUBLES_EQUAL(0, F(0), 1e-9); // check first value is 0
+  Vector points = Chebyshev2::Points(N, a, b);
+  Vector ramp(N);
+  for (size_t i = 0; i < N; ++i) ramp(i) = points(i) - a;
+  EXPECT(assert_equal(ramp, F, 1e-9));
+
+  // Get values of the derivative (fprime) at the Chebyshev nodes
+  Vector fp = Chebyshev2::vector(fprime, N, a, b);
+
+  // Integrate to get back f, using the integration matrix.
+  // Since there is a constant term, we need to add it back.
+  Vector F_est = P * fp;
+  EXPECT_DOUBLES_EQUAL(0, F_est(0), 1e-9); // check first value is 0
+
+  // For comparison, get actual function values at the nodes
+  Vector F_true = Chebyshev2::vector(f, N, a, b);
+
+  // Verify the integration matrix worked correctly, after adding back the
+  // constant term
+  F_est.array() += f(a);
+  EXPECT(assert_equal(F_true, F_est, 1e-9));
+
+  // Differentiate the result to get back to our derivative function
+  Matrix D = Chebyshev2::DifferentiationMatrix(N, a, b);
+  Vector ff_est = D * F_est;
+
+  // Verify the round trip worked
+  EXPECT(assert_equal(fp, ff_est, 1e-9));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, IntegrationWeights7) {
+  const size_t N = 7;
+  Weights actual = Chebyshev2::IntegrationWeights(N, -1, 1);
+
+  // Expected values were calculated using chebfun:
+  Weights expected = (Weights(N) << 0.0285714285714286, 0.253968253968254,
+    0.457142857142857, 0.520634920634921, 0.457142857142857,
+    0.253968253968254, 0.0285714285714286)
+    .finished();
   EXPECT(assert_equal(expected, actual));
 
-  const size_t N8 = 8;
+  // Assert that multiplying with all ones gives the correct integral (2.0)
-  Vector actual2 = Chebyshev2::IntegrationWeights(N8);
+  EXPECT_DOUBLES_EQUAL(2.0, actual.array().sum(), 1e-9);
-  Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208,
+
-                      0.352242423718159, 0.437208405798326, 0.437208405798326,
+  // Integrating f' over [-1,1] should give f(1) - f(-1)
-                      0.352242423718159, 0.190141007218208, 0.0204081632653061)
+  Vector fp = Chebyshev2::vector(fprime, N);
-                         .finished();
+  double expectedF = f(1) - f(-1);
-  EXPECT(assert_equal(expected2, actual2));
+  double actualW = actual * fp;
+  EXPECT_DOUBLES_EQUAL(expectedF, actualW, 1e-9);
+
+  // We can calculate an alternate set of weights using the integration matrix:
+  Matrix P = Chebyshev2::IntegrationMatrix(N);
+  Weights p7 = P.row(N-1);
+
+  // Check that the two sets of weights give the same results
+  EXPECT_DOUBLES_EQUAL(expectedF, p7 * fp, 1e-9);
+
+  // And same for integrate f itself:
+  Vector fvals = Chebyshev2::vector(f, N);
+  EXPECT_DOUBLES_EQUAL(p7*fvals, actual * fvals, 1e-9);
+}
+
+// Check N=8
+TEST(Chebyshev2, IntegrationWeights8) {
+  const size_t N = 8;
+  Weights actual = Chebyshev2::IntegrationWeights(N, -1, 1);
+  Weights expected = (Weights(N) << 0.0204081632653061, 0.190141007218208,
+    0.352242423718159, 0.437208405798326, 0.437208405798326,
+    0.352242423718159, 0.190141007218208, 0.0204081632653061)
+    .finished();
+  EXPECT(assert_equal(expected, actual));
+  EXPECT_DOUBLES_EQUAL(2.0, actual.array().sum(), 1e-9);
+}
+
+//******************************************************************************
+TEST(Chebyshev2, DoubleIntegrationWeights) {
+  const size_t N = 7;
+  const double a = 0, b = 10;
+  // Let's integrate constant twice get a test case:
+  Matrix P = Chebyshev2::IntegrationMatrix(N, a, b);
+  auto ones = Vector::Ones(N);
+  Vector ramp = P * ones;
+  Vector quadratic = P * ramp;
+  
+  // Check the sum which should be 0.5*t^2 | [0,b] = b^2/2:
+  Weights w = Chebyshev2::DoubleIntegrationWeights(N, a, b);
+  EXPECT_DOUBLES_EQUAL(b*b/2, w * ones, 1e-9);
+}
+
+TEST(Chebyshev2, DoubleIntegrationWeights2) {
+  const size_t N = 8;
+  const double a = 0, b = 3;
+  // Let's integrate constant twice get a test case:
+  Matrix P = Chebyshev2::IntegrationMatrix(N, a, b);
+  auto ones = Vector::Ones(N);
+  Vector ramp = P * ones;
+  Vector quadratic = P * ramp;
+  
+  // Check the sum which should be 0.5*t^2 | [0,b] = b^2/2:
+  Weights w = Chebyshev2::DoubleIntegrationWeights(N, a, b);
+  EXPECT_DOUBLES_EQUAL(b*b/2, w * ones, 1e-9);
 }
 
 //******************************************************************************