From 5ef8c0ae1a8748b21ada3bfb7533d2c73f49dd39 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Mar 2025 14:17:40 -0400 Subject: [PATCH] IntegrationMatrix and DoubleIntegrationWeights --- gtsam/basis/Chebyshev2.cpp | 79 +++++++--- gtsam/basis/Chebyshev2.h | 23 ++- gtsam/basis/tests/testChebyshev2.cpp | 209 +++++++++++++++++---------- 3 files changed, 210 insertions(+), 101 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index be8ac5217..137b3895b 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -17,6 +17,9 @@ */ #include + +#include + #include 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) { - // we reverse the sign since we order the cheb points from -1 to 1 - row(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 - row(j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; - } - else if (i == j) { - double xi2 = xi * xi; - row(j) = -xi / (2 * (1 - xi2)); + 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 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 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) { - // Obtain the differentiation matrix. - Matrix D = DifferentiationMatrix(N); +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); +} - // We want f = D * F, where F is the anti-derivative of f. - // However, D is singular, so we enforce F(0) = f(0) by modifying its first row. - D.row(0).setZero(); - D(0, 0) = 1.0; - - // Now D is invertible; its inverse is the integration operator. - return D.inverse(); +Weights Chebyshev2::DoubleIntegrationWeights(size_t N, double a, double b) { + return Chebyshev2::IntegrationWeights(N, a, b) * Chebyshev2::IntegrationMatrix(N, a, b); } /** diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 1cb2a8da2..d1c269e44 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -102,19 +102,32 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { /// 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. - static Matrix IntegrationMatrix(size_t N); + /** + * 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 f, diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index b83f6e789..98003d937 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -9,13 +9,13 @@ * -------------------------------------------------------------------------- */ -/** - * @file testChebyshev2.cpp - * @date July 4, 2020 - * @author Varun Agrawal - * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral - * methods - */ + /** + * @file testChebyshev2.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral + * methods + */ #include #include @@ -28,15 +28,8 @@ #include #include -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; @@ -121,16 +114,17 @@ TEST(Chebyshev2, InterpolateVector) { // Check derivative std::function f = - std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, - std::placeholders::_1, nullptr); + std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11(f, X); + numericalDerivative11(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 f = - std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, - std::placeholders::_1, nullptr); + std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11(f, X); + numericalDerivative11(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 f = - std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, - std::placeholders::_1, nullptr); + std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11(f, X); + numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-8)); } #endif @@ -197,7 +192,7 @@ TEST(Chebyshev2, Decomposition) { } // Do Chebyshev Decomposition - FitBasis actual(sequence, model, 3); + FitBasis 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, 2.0000, -1.5000; + 0.5000, -0.0000, -0.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, // - -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.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // - 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; + 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.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // + -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // + 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,7 +250,8 @@ double fprime(double x) { //****************************************************************************** TEST(Chebyshev2, CalculateWeights) { -Vector fvals = Chebyshev2::vector(f, N); + const size_t N = 32; + Vector fvals = Chebyshev2::vector(f, N); double x1 = 0.7, x2 = -0.376; Weights weights1 = Chebyshev2::CalculateWeights(N, x1); Weights weights2 = Chebyshev2::CalculateWeights(N, x2); @@ -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,8 +288,9 @@ TEST(Chebyshev2, CalculateWeights_CoincidingPoint) { } TEST(Chebyshev2, DerivativeWeights) { + const size_t N = 32; Vector fvals = Chebyshev2::vector(f, N); - std::vector testPoints = {0.7, -0.376, 0.0}; + std::vector 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); @@ -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); @@ -416,15 +415,15 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); // Test Jacobian - Matrix expectedH = numericalDerivative11( - std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); + Matrix expectedH = numericalDerivative11( + 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); @@ -447,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( - std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); + Matrix expectedH = numericalDerivative11( + std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -464,60 +463,124 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { Matrix actualH(1, M * N); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); - Matrix expectedH = numericalDerivative11( - std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); + Matrix expectedH = numericalDerivative11( + std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } //****************************************************************************** -TEST(Chebyshev2, IntegralWeights) { - const size_t N7 = 7; - Vector actual = Chebyshev2::IntegrationWeights(N7, -1, 1); - Vector expected = (Vector(N7) << 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; - Vector actual2 = Chebyshev2::IntegrationWeights(N8, -1, 1); - Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, - 0.352242423718159, 0.437208405798326, 0.437208405798326, - 0.352242423718159, 0.190141007218208, 0.0204081632653061) - .finished(); - EXPECT(assert_equal(expected2, actual2)); -} - -//****************************************************************************** -TEST(Chebyshev2, IntegrationMatrixOperator) { +TEST(Chebyshev2, IntegrationMatrix) { const int N = 10; // number of intervals => N+1 nodes - const double a = -1.0, b = 1.0; + const double a = 0, b = 10; // Create integration matrix - Matrix P = Chebyshev2::IntegrationMatrix(N); + 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 ff = Chebyshev2::vector(fprime, N, a, b); + Vector fp = Chebyshev2::vector(fprime, N, a, b); - // Integrate to get back f, using the integration matrix - Vector F_est = P * ff; - - // Assert that the first value is ff(0) - EXPECT_DOUBLES_EQUAL(ff(0), F_est(0), 1e-9); + // 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 - F_est.array() += F_true(0) - F_est(0); + // 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); + Matrix D = Chebyshev2::DifferentiationMatrix(N, a, b); Vector ff_est = D * F_est; // Verify the round trip worked - EXPECT(assert_equal(ff, ff_est, 1e-9)); + 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)); + + // Assert that multiplying with all ones gives the correct integral (2.0) + EXPECT_DOUBLES_EQUAL(2.0, actual.array().sum(), 1e-9); + + // 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(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); } //******************************************************************************