12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2069 from borglab/feature/faster_chebs 

Faster Chebyshev2
release/4.3a0
Frank Dellaert 2025-03-23 18:24:40 -04:00 committed by GitHub
parent fae071d603 f75e41be0e
commit 82a516a40b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 436 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

303
gtsam/basis/Chebyshev2.cpp
View File

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

89
gtsam/basis/Chebyshev2.h
View File

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

199
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -9,7 +9,7 @@
* -------------------------------------------------------------------------- */
/**
/**
* @file testChebyshev2.cpp
* @date July 4, 2020
* @author Varun Agrawal
@ -28,18 +28,11 @@
#include <cstddef>
#include <functional>
using namespace std;
using namespace gtsam;
namespace {
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
const size_t N = 32;
} // namespace
//******************************************************************************
TEST(Chebyshev2, Point) {
static const int N = 5;
static const size_t N = 5;
auto points = Chebyshev2::Points(N);
Vector expected(N);
expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.;
@ -57,7 +50,7 @@ TEST(Chebyshev2, Point) {
//******************************************************************************
TEST(Chebyshev2, PointInInterval) {
static const int N = 5;
static const size_t N = 5;
auto points = Chebyshev2::Points(N, 0, 20);
Vector expected(N);
expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.;
@ -77,7 +70,7 @@ TEST(Chebyshev2, PointInInterval) {
//******************************************************************************
// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5]
TEST(Chebyshev2, Interpolate2) {
size_t N = 3;
const size_t N = 3;
Chebyshev2::EvaluationFunctor fx(N, 0.5);
Vector f(N);
f << 4, 2, 6;
@ -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,10 +250,8 @@ double fprime(double x) {
//******************************************************************************
TEST(Chebyshev2, CalculateWeights) {
Eigen::Matrix<double, -1, 1> fvals(N);
for (size_t i = 0; i < N; i++) {
fvals(i) = f(Chebyshev2::Point(N, i));
}
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);
@ -267,12 +260,9 @@ TEST(Chebyshev2, CalculateWeights) {
}
TEST(Chebyshev2, CalculateWeights2) {
const size_t N = 32;
double a = 0, b = 10, x1 = 7, x2 = 4.12;
Eigen::Matrix<double, -1, 1> fvals(N);
for (size_t i = 0; i < N; i++) {
fvals(i) = f(Chebyshev2::Point(N, i, 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);
@ -283,34 +273,39 @@ TEST(Chebyshev2, CalculateWeights2) {
EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8);
}
TEST(Chebyshev2, DerivativeWeights) {
Eigen::Matrix<double, -1, 1> 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) {
const size_t N = 32;
Vector fvals = Chebyshev2::vector(f, N);
std::vector<double> testPoints = { 0.7, -0.376, 0.0 };
for (double x : testPoints) {
Weights dWeights = Chebyshev2::DerivativeWeights(N, x);
EXPECT_DOUBLES_EQUAL(fprime(x), dWeights * fvals, 1e-9);
}
Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3);
EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9);
// test if derivative calculation and 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);
}
TEST(Chebyshev2, DerivativeWeights2) {
const size_t N = 32;
double x1 = 5, x2 = 4.12, a = 0, b = 10;
Eigen::Matrix<double, -1, 1> fvals(N);
for (size_t i = 0; i < N; i++) {
fvals(i) = f(Chebyshev2::Point(N, i, 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);
@ -318,12 +313,13 @@ TEST(Chebyshev2, DerivativeWeights2) {
Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b);
EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8);
// test if derivative calculation and Chebyshev point is correct
// test if derivative calculation at Chebyshev point is correct
double x3 = Chebyshev2::Point(N, 3, a, b);
Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b);
EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8);
}
//******************************************************************************
// Check two different ways to calculate the derivative weights
TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) {
@ -366,9 +362,8 @@ double proxy3(double x) {
return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points);
}
// Check Derivative evaluation at point x=0.2
TEST(Chebyshev2, Derivative6) {
// Check Derivative evaluation at point x=0.2
// calculate expected values by numerical derivative of synthesis
const double x = 0.2;
Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy3, x);
@ -420,7 +415,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
// Test Jacobian
Matrix expectedH = numericalDerivative11<Vector2, Matrix, M * N>(
Matrix expectedH = numericalDerivative11<Vector2, Matrix, M* N>(
std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}
@ -428,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);
@ -451,7 +446,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
Matrix actualH(M, M * N);
VecD vecd(M, N, points(0), 0, T);
vecd(X, actualH);
Matrix expectedH = numericalDerivative11<Vector1, Matrix, M * N>(
Matrix expectedH = numericalDerivative11<Vector1, Matrix, M* N>(
std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-6));
}
@ -468,28 +463,124 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
Matrix actualH(1, M * N);
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Matrix, M * N>(
Matrix expectedH = numericalDerivative11<double, Matrix, M* N>(
std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}
//******************************************************************************
TEST(Chebyshev2, IntegralWeights) {
const size_t N7 = 7;
Vector actual = Chebyshev2::IntegrationWeights(N7);
Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254,
TEST(Chebyshev2, IntegrationMatrix) {
const size_t N = 10; // number of intervals => N+1 nodes
const double a = 0, b = 10;
// 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 (size_t i = 0; i < N; ++i) ramp(i) = points(i) - a;
EXPECT(assert_equal(ramp, F, 1e-9));
// Get values of the derivative (fprime) at the Chebyshev nodes
Vector fp = Chebyshev2::vector(fprime, N, a, b);
// Integrate to get back f, using the integration matrix.
// Since there is a constant term, we need to add it back.
Vector F_est = P * fp;
EXPECT_DOUBLES_EQUAL(0, F_est(0), 1e-9); // check first value is 0
// For comparison, get actual function values at the nodes
Vector F_true = Chebyshev2::vector(f, N, a, b);
// Verify the integration matrix worked correctly, after adding back the
// constant term
F_est.array() += f(a);
EXPECT(assert_equal(F_true, F_est, 1e-9));
// Differentiate the result to get back to our derivative function
Matrix D = Chebyshev2::DifferentiationMatrix(N, a, b);
Vector ff_est = D * F_est;
// Verify the round trip worked
EXPECT(assert_equal(fp, ff_est, 1e-9));
}
//******************************************************************************
TEST(Chebyshev2, IntegrationWeights7) {
const size_t N = 7;
Weights actual = Chebyshev2::IntegrationWeights(N, -1, 1);
// Expected values were calculated using chebfun:
Weights expected = (Weights(N) << 0.0285714285714286, 0.253968253968254,
0.457142857142857, 0.520634920634921, 0.457142857142857,
0.253968253968254, 0.0285714285714286)
.finished();
EXPECT(assert_equal(expected, actual));
const size_t N8 = 8;
Vector actual2 = Chebyshev2::IntegrationWeights(N8);
Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208,
// 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(expected2, actual2));
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);
}
//******************************************************************************