From ac4959d1fe5296f50608670fa52fbf39bcf66c7b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Mar 2025 15:49:46 -0400 Subject: [PATCH 1/9] Save half the cos() evals for Points --- gtsam/basis/Chebyshev2.cpp | 40 +++++++++++++++++++++++++++++++++----- gtsam/basis/Chebyshev2.h | 27 ++++++++++++------------- 2 files changed, 47 insertions(+), 20 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index df7bf5f19..0e97590e0 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -21,12 +21,42 @@ 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); - // 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; + const double dtheta = M_PI / (N - 1); + return -cos(dtheta * j); +} + +double Chebyshev2::Point(size_t N, int j, double a, double b) { + if (N == 1) return (a + b) / 2; + return a + (b - a) * (Point(N, j) + 1.0) / 2.0; +} + +Vector Chebyshev2::Points(size_t N) { + Vector points(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; +} + +Vector Chebyshev2::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; } Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 207f4b617..a66de20d5 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -51,9 +51,17 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { using Parameters = Eigen::Matrix; using DiffMatrix = Eigen::Matrix; + /** + * @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 { * @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) { - Vector points(N); - for (size_t j = 0; j < N; j++) { - points(j) = Point(N, j); - } - return points; - } + static Vector Points(size_t N); /// All Chebyshev points, within [a,b] interval - 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; - } + static Vector Points(size_t N, double a, double b); /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) From 94590a2492fc411a64d30648db0e856cbb9f4006 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Mar 2025 16:43:54 -0400 Subject: [PATCH 2/9] Add test for coinciding point and avoid copy/pasta --- gtsam/basis/tests/testChebyshev2.cpp | 60 +++++++++++++++------------- 1 file changed, 32 insertions(+), 28 deletions(-) diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 780714936..8e8d88239 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -247,6 +247,13 @@ double f(double x) { return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; } +Eigen::Matrix calculateFvals(size_t N, double a = -1., double b = 1.) { + const Vector xs = Chebyshev2::Points(N, a, b); + Vector fvals(N); + std::transform(xs.data(), xs.data() + N, fvals.data(), f); + return fvals; +} + // its derivative double fprime(double x) { // return 9*(x**2) - 4*(x) + 5 @@ -255,10 +262,7 @@ double fprime(double x) { //****************************************************************************** TEST(Chebyshev2, CalculateWeights) { - Eigen::Matrix fvals(N); - for (size_t i = 0; i < N; i++) { - fvals(i) = f(Chebyshev2::Point(N, i)); - } +Vector fvals = calculateFvals(N); double x1 = 0.7, x2 = -0.376; Weights weights1 = Chebyshev2::CalculateWeights(N, x1); Weights weights2 = Chebyshev2::CalculateWeights(N, x2); @@ -268,11 +272,7 @@ TEST(Chebyshev2, CalculateWeights) { TEST(Chebyshev2, CalculateWeights2) { double a = 0, b = 10, x1 = 7, x2 = 4.12; - - Eigen::Matrix fvals(N); - for (size_t i = 0; i < N; i++) { - fvals(i) = f(Chebyshev2::Point(N, i, a, b)); - } + Vector fvals = calculateFvals(N, a, b); Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); @@ -283,22 +283,29 @@ TEST(Chebyshev2, CalculateWeights2) { EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); } -TEST(Chebyshev2, DerivativeWeights) { - Eigen::Matrix fvals(N); - for (size_t i = 0; i < N; i++) { - fvals(i) = f(Chebyshev2::Point(N, i)); +// Test CalculateWeights when a point coincides with a Chebyshev point +TEST(Chebyshev2, CalculateWeights_CoincidingPoint) { + const size_t N = 5; + 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); - EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); +TEST(Chebyshev2, DerivativeWeights) { + Vector fvals = calculateFvals(N); + 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); + } - Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); - EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); - - // test if derivative calculation and cheb point is correct + // test if derivative calculation at 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); @@ -306,11 +313,7 @@ TEST(Chebyshev2, DerivativeWeights) { TEST(Chebyshev2, DerivativeWeights2) { double x1 = 5, x2 = 4.12, a = 0, b = 10; - - Eigen::Matrix fvals(N); - for (size_t i = 0; i < N; i++) { - fvals(i) = f(Chebyshev2::Point(N, i, a, b)); - } + Vector fvals = calculateFvals(N, a, b); Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); @@ -318,12 +321,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) { From 44e692c3e9481d33c5961bf539afe2807ce7a64b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Mar 2025 16:44:21 -0400 Subject: [PATCH 3/9] use helpers coincidentPoint and signedDistances --- gtsam/basis/Chebyshev2.cpp | 144 +++++++++++++++++++------------------ 1 file changed, 76 insertions(+), 68 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 0e97590e0..fa65763d7 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -24,8 +24,8 @@ namespace gtsam { 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); - return -cos(dtheta * j); + const double dTheta = M_PI / (N - 1); + return -cos(dTheta * j); } double Chebyshev2::Point(size_t N, int j, double a, double b) { @@ -40,9 +40,9 @@ Vector Chebyshev2::Points(size_t N) { return points; } size_t half = N / 2; - const double dtheta = M_PI / (N - 1); + const double dTheta = M_PI / (N - 1); for (size_t j = 0; j < half; ++j) { - double c = cos(j * dtheta); + double c = cos(j * dTheta); points(j) = -c; points(N - 1 - j) = c; } @@ -59,25 +59,49 @@ Vector Chebyshev2::Points(size_t N, double a, double b) { return points; } -Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { - // Allocate space for weights - Weights weights(N); - - // 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 - weights.setZero(); - weights(j) = 1; - return weights; +namespace { + // Find the index of the Chebyshev point that coincides with x + // within the interval [a, b]. If no such point exists, return nullopt. + static std::optional coincidentPoint(size_t N, double x, double a, double b, double tol = 1e-12) { + if (N == 0) return std::nullopt; + if (N == 1) { + double mid = (a + b) / 2; + if (std::abs(x - mid) < tol) return 0; } - distances(j) = dj; + 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(std::round(jCandidate)); + if (std::abs(jCandidate - jRounded) < tol) return jRounded; + } + return std::nullopt; + } + + // Get signed distances from x to all Chebyshev points + static Vector signedDistances(size_t N, double x, double a, double b) { + Vector result(N); + const Vector points = Chebyshev2::Points(N, a, b); // only thing that depends on [a,b] + for (size_t j = 0; j < N; j++) { + const double dj = x - points[j]; + result(j) = dj; + } + return result; + } +} + +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 @@ -99,46 +123,32 @@ 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 Weights weightDerivatives(N); - - // 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); - } + if (auto j = coincidentPoint(N, x, a, b)) { + // 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.; + double t = ((*j + k) % 2) == 0 ? 1 : -1; + weightDerivatives(k) = (cj / ck) * t / (xj - xk); } - return 2 * weightDerivatives / (b - a); } - distances(j) = dj; + return 2 * weightDerivatives / (b - a); } // This section of code computes the derivative of @@ -150,6 +160,7 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { double g = 0, k = 0; double w = 1; + 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; @@ -157,7 +168,7 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { w = 1.0; } - t = (j % 2 == 0) ? 1 : -1; + double t = (j % 2 == 0) ? 1 : -1; double c = t / distances(j); g += w * c; @@ -183,19 +194,16 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { return weightDerivatives; } -Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, - double b) { +Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, 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 - double t = -1; - + const Vector points = Points(N); // a,b dependence is done at return for (size_t i = 0; i < N; i++) { - double xi = Point(N, i); + 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) { @@ -208,9 +216,9 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, double xi2 = xi * xi; D(i, j) = -xi / (2 * (1 - xi2)); } else { - double xj = Point(N, j); + double xj = points(j); double cj = (j == 0 || j == N - 1) ? 2. : 1.; - t = ((i + j) % 2) == 0 ? 1 : -1; + double t = ((i + j) % 2) == 0 ? 1 : -1; D(i, j) = (ci / cj) * t / (xi - xj); } } From 5ffa9c1788a15c1489a9e32a3c1ee41a97a5a8e5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Mar 2025 16:57:32 -0400 Subject: [PATCH 4/9] Avoid half the cosines in IntegrationWeights --- gtsam/basis/Chebyshev2.cpp | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index fa65763d7..015a1ec47 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -228,24 +228,26 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, dou } 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 - K2 = K * K; + K2 = K * K; + + // Compute endpoint weight. 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) { + // Compute up to the middle; mirror symmetry holds. + size_t mid = (N - 1) / 2; + for (size_t i = 1; i <= mid; ++i) { double theta = i * M_PI / K; - weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; - + double w = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; + size_t last_k = K / 2 + K % 2 - 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; + w -= 2 * cos(2 * k * theta) / (4 * k * k - 1); + w *= (b - a) / K; + weights(i) = w; + weights(N - 1 - i) = w; } - return weights; } From 392abd6eab436dcb9971f9cd1acf65348a09f352 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Mar 2025 17:29:00 -0400 Subject: [PATCH 5/9] Make Clenshaw-Curtis weights faster in case [-1,1] --- gtsam/basis/Chebyshev2.cpp | 22 +++++++++++++--------- gtsam/basis/Chebyshev2.h | 19 ++++--------------- gtsam/basis/tests/testChebyshev2.cpp | 4 ++-- 3 files changed, 19 insertions(+), 26 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 015a1ec47..fae26eedc 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -227,28 +227,32 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, dou return D / ((b - a) / 2.0); } -Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { +Weights Chebyshev2::IntegrationWeights(size_t N) { Weights weights(N); - size_t K = N - 1, // number of intervals between N points + const size_t K = N - 1, // number of intervals between N points K2 = K * K; // Compute endpoint weight. - weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); + weights(0) = 1.0 / (K2 + K % 2 - 1); weights(N - 1) = weights(0); // Compute up to the middle; mirror symmetry holds. - size_t mid = (N - 1) / 2; + const size_t mid = (N - 1) / 2; + double dTheta = M_PI / K; for (size_t i = 1; i <= mid; ++i) { - double theta = i * M_PI / K; - double w = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; - size_t last_k = K / 2 + K % 2 - 1; + 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 * cos(2 * k * theta) / (4 * k * k - 1); - w *= (b - a) / 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; +} } // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index a66de20d5..124e65b7c 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -105,22 +105,11 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { /** * Evaluate 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); + + /// Evaluate 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. diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 8e8d88239..1702d7a62 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -480,7 +480,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { //****************************************************************************** TEST(Chebyshev2, IntegralWeights) { const size_t N7 = 7; - Vector actual = Chebyshev2::IntegrationWeights(N7); + 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) @@ -488,7 +488,7 @@ TEST(Chebyshev2, IntegralWeights) { EXPECT(assert_equal(expected, actual)); const size_t N8 = 8; - Vector actual2 = Chebyshev2::IntegrationWeights(N8); + 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) From 9d79215fda867194afdd7afabea260424f1e21f8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Mar 2025 17:31:22 -0400 Subject: [PATCH 6/9] Helper differentiationMatrixRow cuts down on copy/pasta --- gtsam/basis/Chebyshev2.cpp | 124 ++++++++++++++++++------------------- gtsam/basis/Chebyshev2.h | 16 ++--- 2 files changed, 69 insertions(+), 71 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index fae26eedc..3a9e68364 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -62,13 +62,12 @@ Vector Chebyshev2::Points(size_t N, double a, double b) { namespace { // Find the index of the Chebyshev point that coincides with x // within the interval [a, b]. If no such point exists, return nullopt. - static std::optional coincidentPoint(size_t N, double x, double a, double b, double tol = 1e-12) { + std::optional coincidentPoint(size_t N, double x, double a, double b, double tol = 1e-12) { if (N == 0) return std::nullopt; if (N == 1) { double mid = (a + b) / 2; if (std::abs(x - mid) < tol) return 0; - } - else { + } 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; @@ -81,16 +80,44 @@ namespace { } // Get signed distances from x to all Chebyshev points - static Vector signedDistances(size_t N, double x, double a, double b) { + Vector signedDistances(size_t N, double x, double a, double b) { Vector result(N); - const Vector points = Chebyshev2::Points(N, a, b); // only thing that depends on [a,b] + 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); + 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)); + } + else { + double xj = points(j); + double cj = (j == 0 || j == N - 1) ? 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 @@ -123,43 +150,20 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { } Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { - Weights weightDerivatives(N); if (auto j = coincidentPoint(N, x, a, b)) { // 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.; - double t = ((*j + k) % 2) == 0 ? 1 : -1; - weightDerivatives(k) = (cj / ck) * t / (xj - xk); - } - } - return 2 * weightDerivatives / (b - a); + return differentiationMatrixRow(N, Points(N), *j) / ((b - a) / 2.0); } - + // 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) { @@ -167,18 +171,19 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { } else { w = 1.0; } - + 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) { @@ -189,11 +194,26 @@ 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) { + DiffMatrix D(N, N); + if (N == 1) { + D(0, 0) = 1; + return D; + } + + const Vector points = Points(N); + for (size_t i = 0; i < N; i++) { + D.row(i) = differentiationMatrixRow(N, points, i); + } + return D; +} + Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, double b) { DiffMatrix D(N, N); if (N == 1) { @@ -201,30 +221,8 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, dou return D; } - const Vector points = Points(N); // a,b dependence is done at return - for (size_t i = 0; i < N; i++) { - 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 - 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 = points(j); - double cj = (j == 0 || j == N - 1) ? 2. : 1.; - double t = ((i + j) % 2) == 0 ? 1 : -1; - D(i, j) = (ci / cj) * t / (xi - xj); - } - } - } - // scale the matrix to the range - return D / ((b - a) / 2.0); + // Calculate for [-1,1] and scale for [a,b] + return DifferentiationMatrix(N) / ((b - a) / 2.0); } Weights Chebyshev2::IntegrationWeights(size_t N) { diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 124e65b7c..6486de0cc 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -85,22 +85,22 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * 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, - double b = 1); + static Weights CalculateWeights(size_t N, double x, double a = -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, - double b = 1); + static Weights DerivativeWeights(size_t N, double x, double a = -1, double b = 1); - /// compute D = differentiation matrix, Trefethen00book p.53 - /// when given a parameter vector f of function values at the Chebyshev + /// Compute D = differentiation matrix, Trefethen00book p.53 + /// 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, - double b = 1); + static DiffMatrix DifferentiationMatrix(size_t N); + + /// Compute D = differentiation matrix, for interval [a,b] + static DiffMatrix DifferentiationMatrix(size_t N, double a, double b); /** * Evaluate Clenshaw-Curtis integration weights. From e32ccdfec70906ac98d8deff8a4a19110ea6cb1c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Mar 2025 23:09:30 -0400 Subject: [PATCH 7/9] IntegrationMatrix --- gtsam/basis/Chebyshev2.cpp | 24 +++++++++++++ gtsam/basis/Chebyshev2.h | 20 +++++++---- gtsam/basis/tests/testChebyshev2.cpp | 52 ++++++++++++++++++++-------- 3 files changed, 75 insertions(+), 21 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 3a9e68364..be8ac5217 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -253,4 +253,28 @@ Weights Chebyshev2::IntegrationWeights(size_t N) { 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); + + // 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(); +} + +/** + * Create vector of values at Chebyshev points given scalar-valued function. + */ +Vector Chebyshev2::vector(std::function 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 diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 6486de0cc..1cb2a8da2 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -105,22 +105,28 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { /** * Evaluate 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] static Weights IntegrationWeights(size_t N, double a, double b); - /** - * Create matrix of values at Chebyshev points given vector-valued function. - */ + /// 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); + + /// Create matrix of values at Chebyshev points given vector-valued function. + static Vector vector(std::function f, + size_t N, double a = -1, double b = 1); + + /// Create matrix of values at Chebyshev points given vector-valued function. template static Matrix matrix(std::function(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++) { - Xmat.col(j) = f(Point(N, j, a, b)); - } + const Vector points = Points(N, a, b); + for (size_t j = 0; j < N; j++) Xmat.col(j) = f(points(j)); return Xmat; } }; // \ Chebyshev2 diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 1702d7a62..b83f6e789 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -247,13 +247,6 @@ double f(double x) { return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; } -Eigen::Matrix calculateFvals(size_t N, double a = -1., double b = 1.) { - const Vector xs = Chebyshev2::Points(N, a, b); - Vector fvals(N); - std::transform(xs.data(), xs.data() + N, fvals.data(), f); - return fvals; -} - // its derivative double fprime(double x) { // return 9*(x**2) - 4*(x) + 5 @@ -262,7 +255,7 @@ double fprime(double x) { //****************************************************************************** TEST(Chebyshev2, CalculateWeights) { -Vector fvals = calculateFvals(N); +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); @@ -272,7 +265,7 @@ Vector fvals = calculateFvals(N); TEST(Chebyshev2, CalculateWeights2) { double a = 0, b = 10, x1 = 7, x2 = 4.12; - Vector fvals = calculateFvals(N, a, b); + Vector fvals = Chebyshev2::vector(f, N, a, b); Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); @@ -298,14 +291,14 @@ TEST(Chebyshev2, CalculateWeights_CoincidingPoint) { } TEST(Chebyshev2, DerivativeWeights) { - Vector fvals = calculateFvals(N); + Vector fvals = Chebyshev2::vector(f, N); 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); } - // test if derivative calculation at cheb point is correct + // test if derivative calculation at Chebyshev point is correct double x4 = Chebyshev2::Point(N, 3); Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); @@ -313,7 +306,7 @@ TEST(Chebyshev2, DerivativeWeights) { TEST(Chebyshev2, DerivativeWeights2) { double x1 = 5, x2 = 4.12, a = 0, b = 10; - Vector fvals = calculateFvals(N, a, b); + Vector fvals = Chebyshev2::vector(f, N, a, b); Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); @@ -370,9 +363,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(proxy3, x); @@ -496,6 +488,38 @@ TEST(Chebyshev2, IntegralWeights) { EXPECT(assert_equal(expected2, actual2)); } +//****************************************************************************** +TEST(Chebyshev2, IntegrationMatrixOperator) { + const int N = 10; // number of intervals => N+1 nodes + const double a = -1.0, b = 1.0; + + // Create integration matrix + Matrix P = Chebyshev2::IntegrationMatrix(N); + + // Get values of the derivative (fprime) at the Chebyshev nodes + Vector ff = 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); + + // 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)); +} + //****************************************************************************** int main() { TestResult tr; From 5ef8c0ae1a8748b21ada3bfb7533d2c73f49dd39 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Mar 2025 14:17:40 -0400 Subject: [PATCH 8/9] 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); } //****************************************************************************** From f75e41be0eaa5db8c61c9362e4f26ca52e52257c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Mar 2025 17:05:13 -0400 Subject: [PATCH 9/9] int -> size_t --- gtsam/basis/Chebyshev2.cpp | 2 +- gtsam/basis/tests/testChebyshev2.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 137b3895b..c5dc46640 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -232,7 +232,7 @@ Matrix Chebyshev2::IntegrationMatrix(size_t N) { 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); + 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. diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 98003d937..dee75297c 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -32,7 +32,7 @@ using namespace gtsam; //****************************************************************************** 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.; @@ -50,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.; @@ -470,7 +470,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { //****************************************************************************** TEST(Chebyshev2, IntegrationMatrix) { - const int N = 10; // number of intervals => N+1 nodes + const size_t N = 10; // number of intervals => N+1 nodes const double a = 0, b = 10; // Create integration matrix @@ -482,7 +482,7 @@ TEST(Chebyshev2, IntegrationMatrix) { 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; + 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