IntegrationMatrix and DoubleIntegrationWeights

gtsam/basis/Chebyshev2.cpp

@@ -17,6 +17,9 @@
  */
 
 #include <gtsam/basis/Chebyshev2.h>
+
+#include <Eigen/Dense>
+
 #include <cassert>
 
 namespace gtsam {
@@ -93,24 +96,20 @@ namespace {
   // 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);
-    double ci = (i == 0 || i == N - 1) ? 2. : 1.;
     for (size_t j = 0; j < N; j++) {
-      if (i == 0 && j == 0) {
+      if (i == j) {
-        // we reverse the sign since we order the cheb points from -1 to 1
+        // Diagonal elements
-        row(j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0;
+        if (i == 0 || i == K)
-      }
+          row(j) = (i == 0 ? -1 : 1) * (2.0 * K * K + 1) / 6.0;
-      else if (i == N - 1 && j == N - 1) {
+        else
-        // we reverse the sign since we order the cheb points from -1 to 1
+          row(j) = -xi / (2.0 * (1.0 - xi * xi));
-        row(j) = (ci * (N - 1) * (N - 1) + 1) / 6.0;
-      }
-      else if (i == j) {
-        double xi2 = xi * xi;
-        row(j) = -xi / (2 * (1 - xi2));
       }
       else {
         double xj = points(j);
-        double cj = (j == 0 || j == N - 1) ? 2. : 1.;
+        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);
       }
@@ -225,6 +224,43 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, dou
   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 (int 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
@@ -254,17 +290,14 @@ Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
   return IntegrationWeights(N) * (b - a) / 2.0;
 }
 
-Matrix Chebyshev2::IntegrationMatrix(size_t N) {
+Weights Chebyshev2::DoubleIntegrationWeights(size_t N) {
-  // Obtain the differentiation matrix.
+  // we have w * P, where w are the Clenshaw-Curtis weights and P is the integration matrix
-  Matrix D = DifferentiationMatrix(N);
+  // But P does not by default return a function starting at zero.
+  return Chebyshev2::IntegrationWeights(N) * Chebyshev2::IntegrationMatrix(N);
+}
 
-  // We want f = D * F, where F is the anti-derivative of f.  
+Weights Chebyshev2::DoubleIntegrationWeights(size_t N, double a, double b) {
-  // However, D is singular, so we enforce F(0) = f(0) by modifying its first row.
+  return Chebyshev2::IntegrationWeights(N, a, b) * Chebyshev2::IntegrationMatrix(N, a, b);
-  D.row(0).setZero();
-  D(0, 0) = 1.0;
-
-  // Now D is invertible; its inverse is the integration operator.
-  return D.inverse();
 }
 
 /**

gtsam/basis/Chebyshev2.h

@@ -102,19 +102,32 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   /// 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
    */
   static Weights IntegrationWeights(size_t N);
 
-  /// Evaluate Clenshaw-Curtis integration weights, for interval [a,b]
+  /// Calculate Clenshaw-Curtis integration weights, for interval [a,b]
   static Weights IntegrationWeights(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.
+   * Calculate Double Clenshaw-Curtis integration weights
-  static Matrix IntegrationMatrix(size_t N);
+   * 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,

gtsam/basis/tests/testChebyshev2.cpp

@@ -28,15 +28,8 @@
 #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;
@@ -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;
@@ -131,6 +124,7 @@ TEST(Chebyshev2, InterpolateVector) {
 //******************************************************************************
 // 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);
@@ -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);
 
@@ -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);
@@ -255,6 +250,7 @@ double fprime(double x) {
 
 //******************************************************************************
 TEST(Chebyshev2, CalculateWeights) {
+  const size_t N = 32;
   Vector fvals = Chebyshev2::vector(f, N);
   double x1 = 0.7, x2 = -0.376;
   Weights weights1 = Chebyshev2::CalculateWeights(N, x1);
@@ -264,6 +260,7 @@ Vector fvals = Chebyshev2::vector(f, N);
 }
 
 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);
 
@@ -291,6 +288,7 @@ TEST(Chebyshev2, CalculateWeights_CoincidingPoint) {
 }
 
 TEST(Chebyshev2, DerivativeWeights) {
+  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) {
@@ -305,6 +303,7 @@ TEST(Chebyshev2, DerivativeWeights) {
 }
 
 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);
 
@@ -424,7 +423,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
 //******************************************************************************
 // 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);
@@ -470,54 +469,118 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
 }
 
 //******************************************************************************
-TEST(Chebyshev2, IntegralWeights) {
+TEST(Chebyshev2, IntegrationMatrix) {
-  const size_t N7 = 7;
+  const int N = 10;  // number of intervals => N+1 nodes
-  Vector actual = Chebyshev2::IntegrationWeights(N7, -1, 1);
+  const double a = 0, b = 10;
-  Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254,
+
+  // Create integration matrix
+  Matrix P = Chebyshev2::IntegrationMatrix(N, a, b);
+
+  // 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 (int 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, -1, 1);
+  EXPECT_DOUBLES_EQUAL(2.0, actual.array().sum(), 1e-9);
-  Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208,
+
+  // Integrating f' over [-1,1] should give f(1) - f(-1)
+  Vector fp = Chebyshev2::vector(fprime, N);
+  double expectedF = f(1) - f(-1);
+  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(expected2, actual2));
+  EXPECT(assert_equal(expected, actual));
+  EXPECT_DOUBLES_EQUAL(2.0, actual.array().sum(), 1e-9);
 }
 
 //******************************************************************************
-TEST(Chebyshev2, IntegrationMatrixOperator) {
+TEST(Chebyshev2, DoubleIntegrationWeights) {
-  const int N = 10;  // number of intervals => N+1 nodes
+  const size_t N = 7;
-  const double a = -1.0, b = 1.0;
+  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;
   
-  // Create integration matrix
+  // Check the sum which should be 0.5*t^2 | [0,b] = b^2/2:
-  Matrix P = Chebyshev2::IntegrationMatrix(N);
+  Weights w = Chebyshev2::DoubleIntegrationWeights(N, a, b);
+  EXPECT_DOUBLES_EQUAL(b*b/2, w * ones, 1e-9);
+}
 
-  // Get values of the derivative (fprime) at the Chebyshev nodes
+TEST(Chebyshev2, DoubleIntegrationWeights2) {
-  Vector ff = Chebyshev2::vector(fprime, N, a, b);
+  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;
   
-  // Integrate to get back f, using the integration matrix
+  // Check the sum which should be 0.5*t^2 | [0,b] = b^2/2:
-  Vector F_est = P * ff;
+  Weights w = Chebyshev2::DoubleIntegrationWeights(N, a, b);
-
+  EXPECT_DOUBLES_EQUAL(b*b/2, w * ones, 1e-9);
-  // Assert that the first value is ff(0)
-  EXPECT_DOUBLES_EQUAL(ff(0), F_est(0), 1e-9);
-
-  // For comparison, get actual function values at the nodes
-  Vector F_true = Chebyshev2::vector(f, N, a, b);
-
-  // Verify the integration matrix worked correctly
-  F_est.array() += F_true(0) - F_est(0);
-  EXPECT(assert_equal(F_true, F_est, 1e-9));
-
-  // Differentiate the result to get back to our derivative function
-  Matrix D = Chebyshev2::DifferentiationMatrix(N);
-  Vector ff_est = D * F_est;
-
-  // Verify the round trip worked
-  EXPECT(assert_equal(ff, ff_est, 1e-9));
 }
 
 //******************************************************************************