diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 63fca64cc..b8036b635 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -22,13 +22,13 @@ namespace gtsam { Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Allocate space for weights - Weights weights(N); + Weights weights(N + 1); // We start by getting distances from x to all Chebyshev points - // as well as getting smallest distance - Weights distances(N); + // as well as getting the smallest distance + Weights distances(N + 1); - for (size_t j = 0; j < N; j++) { + for (size_t j = 0; j <= N; j++) { const double dj = x - Point(N, j, a, b); // only thing that depends on [a,b] @@ -44,16 +44,16 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Beginning of interval, j = 0, x(0) = a weights(0) = 0.5 / distances(0); - // All intermediate points j=1:N-2 + // All intermediate points j=1:N-1 double d = weights(0), s = -1; // changes sign s at every iteration - for (size_t j = 1; j < N - 1; j++, s = -s) { + for (size_t j = 1; j < N; j++, s = -s) { weights(j) = s / distances(j); d += weights(j); } - // End of interval, j = N-1, x(N-1) = b - weights(N - 1) = 0.5 * s / distances(N - 1); - d += weights(N - 1); + // End of interval, j = N, x(N) = b + weights(N) = 0.5 * s / distances(N); + d += weights(N); // normalize return weights / d; @@ -61,30 +61,30 @@ 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); + Weights weightDerivatives(N + 1); // 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); + Weights distances(N + 1); - for (size_t j = 0; j < N; j++) { + 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++) { + double cj = (j == 0 || j == N) ? 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) { + weightDerivatives(k) = -(cj * N * N + 1) / 6.0; + } else if (j == N && k == N) { // we reverse the sign since we order the cheb points from -1 to 1 - weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; + weightDerivatives(k) = (cj * N * N + 1) / 6.0; } else if (k == j) { double xj = Point(N, j); double xj2 = xj * xj; @@ -92,7 +92,7 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { } else { double xj = Point(N, j); double xk = Point(N, k); - double ck = (k == 0 || k == N - 1) ? 2. : 1.; + double ck = (k == 0 || k == N) ? 2. : 1.; t = ((j + k) % 2) == 0 ? 1 : -1; weightDerivatives(k) = (cj / ck) * t / (xj - xk); } @@ -111,8 +111,8 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { double g = 0, k = 0; double w = 1; - for (size_t j = 0; j < N; j++) { - if (j == 0 || j == N - 1) { + for (size_t j = 0; j <= N; j++) { + if (j == 0 || j == N) { w = 0.5; } else { w = 1.0; @@ -128,13 +128,13 @@ 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) { - // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, - // x0 = 1.0 - if (j == 0 || j == N - 1) { + for (size_t j = 0; j <= N; j++, s = -s) { + // Beginning of interval, j = 0, x0 = -1.0 + // and end of interval, j = N, x0 = 1.0 + if (j == 0 || j == N) { w = 0.5; } else { - // All intermediate points j=1:N-2 + // All intermediate points j=1:N-1 w = 1.0; } weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - @@ -146,8 +146,8 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, double b) { - DiffMatrix D(N, N); - if (N == 1) { + DiffMatrix D(N + 1, N + 1); + if (N + 1 == 1) { D(0, 0) = 1; return D; } @@ -155,22 +155,22 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, // toggle variable so we don't need to use `pow` for -1 double t = -1; - for (size_t i = 0; i < N; i++) { + 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++) { + double ci = (i == 0 || i == N) ? 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) { + D(i, j) = -(ci * N * N + 1) / 6.0; + } else if (i == N && j == N) { // 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; + D(i, j) = (ci * N * N + 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.; + double cj = (j == 0 || j == N) ? 2. : 1.; t = ((i + j) % 2) == 0 ? 1 : -1; D(i, j) = (ci / cj) * t / (xi - xj); } @@ -182,15 +182,15 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, 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 + Weights weights(N + 1); + size_t K = N, // number of intervals between N points K2 = K * K; weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); - weights(N - 1) = weights(0); + weights(N) = weights(0); size_t last_k = K / 2 + K % 2 - 1; - for (size_t i = 1; i <= N - 2; ++i) { + for (size_t i = 1; i <= N - 1; ++i) { double theta = i * M_PI / K; weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 6cc18bccc..f7537eb64 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -62,8 +62,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * @return double */ static double Point(size_t N, int j, double a = -1, double b = 1) { - assert(j >= 0 && size_t(j) < N); - const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + assert(j >= 0 && size_t(j) <= N); + const double dtheta = M_PI / (N > 1 ? N : 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; @@ -71,8 +71,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { /// All Chebyshev points static Vector Points(size_t N) { - Vector points(N); - for (size_t j = 0; j < N; j++) { + Vector points(N + 1); + for (size_t j = 0; j <= N; j++) { points(j) = Point(N, j); } return points; @@ -92,7 +92,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { } /** - * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N (N+1 values) * These weights implement barycentric interpolation at a specific x. * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the * values of the function f at the Chebyshev points. As such, for a given x we @@ -142,8 +142,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { template static Matrix matrix(std::function(double)> f, size_t N, double a = -1, double b = 1) { - Matrix Xmat(M, N); - for (size_t j = 0; j < N; j++) { + Matrix Xmat(M, N + 1); + for (size_t j = 0; j <= N; j++) { Xmat.col(j) = f(Point(N, j, a, b)); } return Xmat; diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index d33e13de6..afe0168fe 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -57,7 +57,7 @@ TEST(BasisFactors, EvaluationFactor) { NonlinearFactorGraph graph; graph.add(factor); - Vector functionValues(N); + Vector functionValues(N + 1); functionValues.setZero(); Values initial; @@ -84,7 +84,7 @@ TEST(BasisFactors, VectorEvaluationFactor) { NonlinearFactorGraph graph; graph.add(factor); - gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N + 1); Values initial; initial.insert(key, stateMatrix); @@ -132,7 +132,7 @@ TEST(BasisFactors, VectorComponentFactor) { NonlinearFactorGraph graph; graph.add(factor); - gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N + 1); Values initial; initial.insert(key, stateMatrix); @@ -157,7 +157,7 @@ TEST(BasisFactors, ManifoldEvaluationFactor) { NonlinearFactorGraph graph; graph.add(factor); - gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N + 1); Values initial; initial.insert(key, stateMatrix); @@ -184,7 +184,7 @@ TEST(BasisFactors, VecDerivativePrior) { NonlinearFactorGraph graph; graph.add(vecDPrior); - gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N + 1); Values initial; initial.insert(key, stateMatrix); @@ -211,7 +211,7 @@ TEST(BasisFactors, ComponentDerivativeFactor) { graph.add(controlDPrior); Values initial; - gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N + 1); initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 780714936..9648ce86e 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -39,9 +39,9 @@ const size_t N = 32; //****************************************************************************** TEST(Chebyshev2, Point) { - static const int N = 5; + static const int N = 4; auto points = Chebyshev2::Points(N); - Vector expected(N); + Vector expected(N + 1); expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; static const double tol = 1e-15; // changing this reveals errors EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); @@ -57,9 +57,9 @@ TEST(Chebyshev2, Point) { //****************************************************************************** TEST(Chebyshev2, PointInInterval) { - static const int N = 5; + static const int N = 4; auto points = Chebyshev2::Points(N, 0, 20); - Vector expected(N); + Vector expected(N + 1); expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; expected *= 10.0; static const double tol = 1e-15; // changing this reveals errors @@ -77,9 +77,9 @@ TEST(Chebyshev2, PointInInterval) { //****************************************************************************** // InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] TEST(Chebyshev2, Interpolate2) { - size_t N = 3; + size_t N = 2; Chebyshev2::EvaluationFunctor fx(N, 0.5); - Vector f(N); + Vector f(N + 1); f << 4, 2, 6; EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); } @@ -87,7 +87,7 @@ TEST(Chebyshev2, Interpolate2) { //****************************************************************************** // InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] TEST(Chebyshev2, Interpolate2_Interval) { - Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); + Chebyshev2::EvaluationFunctor fx(2, 1.5, 0, 2); Vector3 f(4, 2, 6); EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); } @@ -96,7 +96,7 @@ TEST(Chebyshev2, Interpolate2_Interval) { // InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, // 3}}, 0.5] TEST(Chebyshev2, Interpolate5) { - Chebyshev2::EvaluationFunctor fx(5, 0.5); + Chebyshev2::EvaluationFunctor fx(4, 0.5); Vector f(5); f << 4, 2, 6, 3, 3; EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); @@ -108,13 +108,13 @@ TEST(Chebyshev2, InterpolateVector) { double t = 30, a = 0, b = 100; const size_t N = 3; // Create 2x3 matrix with Vectors at Chebyshev points - Matrix X = Matrix::Zero(2, N); + Matrix X = Matrix::Zero(2, N + 1); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp // Check value Vector expected(2); expected << t, 0; - Eigen::Matrix actualH(2, 2 * N); + Eigen::Matrix actualH(2, 2 * (N + 1)); Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); @@ -123,8 +123,7 @@ TEST(Chebyshev2, InterpolateVector) { std::function f = std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); - Matrix numericalH = - numericalDerivative11(f, X); + Matrix numericalH = numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } @@ -133,10 +132,10 @@ TEST(Chebyshev2, InterpolateVector) { TEST(Chebyshev2, InterpolatePose2) { double t = 30, a = 0, b = 100; - Matrix X(3, N); + Matrix X(3, N + 1); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp - X.row(1) = Vector::Zero(N); - X.row(2) = 0.1 * Vector::Ones(N); + X.row(1) = Vector::Zero(N + 1); + X.row(2) = 0.1 * Vector::Ones(N + 1); Vector xi(3); xi << t, 0, 0.1; @@ -151,8 +150,7 @@ TEST(Chebyshev2, InterpolatePose2) { std::function f = std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); - Matrix numericalH = - numericalDerivative11(f, X); + Matrix numericalH = numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } @@ -167,9 +165,9 @@ TEST(Chebyshev2, InterpolatePose3) { Pose3 pose(R, Point3(1, 2, 3)); Vector6 xi = Pose3::ChartAtOrigin::Local(pose); - Eigen::Matrix actualH(6, 6 * N); + Eigen::Matrix actualH(6, 6 * N + 1); - Matrix X = Matrix::Zero(6, N); + Matrix X = Matrix::Zero(6, N + 1); X.col(11) = xi; Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); @@ -181,8 +179,7 @@ TEST(Chebyshev2, InterpolatePose3) { std::function f = std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); - Matrix numericalH = - numericalDerivative11(f, X); + Matrix numericalH = numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-8)); } #endif @@ -200,16 +197,16 @@ TEST(Chebyshev2, Decomposition) { FitBasis actual(sequence, model, 3); // Check - Vector expected(3); - expected << -1, 0, 1; + Vector expected(4); + expected << -1, -0.5, 0.5, 1; EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); } //****************************************************************************** TEST(Chebyshev2, DifferentiationMatrix3) { // Trefethen00book, p.55 - const size_t N = 3; - Matrix expected(N, N); + const size_t N = 2; + Matrix expected(N + 1, N + 1); // Differentiation matrix computed from chebfun expected << 1.5000, -2.0000, 0.5000, // 0.5000, -0.0000, -0.5000, // @@ -225,8 +222,8 @@ TEST(Chebyshev2, DifferentiationMatrix3) { //****************************************************************************** TEST(Chebyshev2, DerivativeMatrix6) { // Trefethen00book, p.55 - const size_t N = 6; - Matrix expected(N, N); + const size_t N = 5; + Matrix expected(N + 1, N + 1); 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, // @@ -238,7 +235,7 @@ TEST(Chebyshev2, DerivativeMatrix6) { expected = -expected; Matrix actual = Chebyshev2::DifferentiationMatrix(N); - EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); + EXPECT(assert_equal(expected, actual, 1e-4)); } // test function for CalculateWeights and DerivativeWeights @@ -255,8 +252,8 @@ double fprime(double x) { //****************************************************************************** TEST(Chebyshev2, CalculateWeights) { - Eigen::Matrix fvals(N); - for (size_t i = 0; i < N; i++) { + Eigen::Matrix fvals(N + 1); + for (size_t i = 0; i <= N; i++) { fvals(i) = f(Chebyshev2::Point(N, i)); } double x1 = 0.7, x2 = -0.376; @@ -269,8 +266,8 @@ 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++) { + Eigen::Matrix fvals(N + 1); + for (size_t i = 0; i <= N; i++) { fvals(i) = f(Chebyshev2::Point(N, i, a, b)); } @@ -284,8 +281,8 @@ TEST(Chebyshev2, CalculateWeights2) { } TEST(Chebyshev2, DerivativeWeights) { - Eigen::Matrix fvals(N); - for (size_t i = 0; i < N; i++) { + Eigen::Matrix fvals(N + 1); + for (size_t i = 0; i <= N; i++) { fvals(i) = f(Chebyshev2::Point(N, i)); } double x1 = 0.7, x2 = -0.376, x3 = 0.0; @@ -307,8 +304,8 @@ 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++) { + Eigen::Matrix fvals(N + 1); + for (size_t i = 0; i <= N; i++) { fvals(i) = f(Chebyshev2::Point(N, i, a, b)); } @@ -347,7 +344,7 @@ TEST(Chebyshev2, DerivativeWeights6) { const size_t N6 = 6; Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 - EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); + EXPECT(assert_equal(Vector::Ones(N6 + 1), Vector(D6 * x))); } //****************************************************************************** @@ -356,14 +353,14 @@ TEST(Chebyshev2, DerivativeWeights7) { const size_t N7 = 7; Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 - EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); + EXPECT(assert_equal(Vector::Ones(N7 + 1), Vector(D7 * x))); } //****************************************************************************** // Check derivative in two different ways: numerical and using D on f Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); double proxy3(double x) { - return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); + return Chebyshev2::EvaluationFunctor(5, x)(f3_at_6points); } TEST(Chebyshev2, Derivative6) { @@ -373,21 +370,23 @@ TEST(Chebyshev2, Derivative6) { const double x = 0.2; Matrix numeric_dTdx = numericalDerivative11(proxy3, x); + size_t N = 5; + // Calculate derivatives at Chebyshev points using D3, interpolate - Matrix D6 = Chebyshev2::DifferentiationMatrix(6); + Matrix D6 = Chebyshev2::DifferentiationMatrix(N); Vector derivative_at_points = D6 * f3_at_6points; - Chebyshev2::EvaluationFunctor fx(6, x); + Chebyshev2::EvaluationFunctor fx(N, x); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); // Do directly - Chebyshev2::DerivativeFunctor dfdx(6, x); + Chebyshev2::DerivativeFunctor dfdx(N, x); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); } //****************************************************************************** // Assert that derivative also works in non-standard interval [0,3] double proxy4(double x) { - return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); + return Chebyshev2::EvaluationFunctor(5, x, 0, 3)(f3_at_6points); } TEST(Chebyshev2, Derivative6_03) { @@ -397,14 +396,16 @@ TEST(Chebyshev2, Derivative6_03) { const double x = 0.2; Matrix numeric_dTdx = numericalDerivative11(proxy4, x); + size_t N = 5; + // Calculate derivatives at Chebyshev points using D3, interpolate - Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); + Matrix D6 = Chebyshev2::DifferentiationMatrix(N, 0, 3); Vector derivative_at_points = D6 * f3_at_6points; - Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); + Chebyshev2::EvaluationFunctor fx(N, x, 0, 3); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); // Do directly - Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); + Chebyshev2::DerivativeFunctor dfdx(N, x, 0, 3); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); } @@ -415,12 +416,12 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { const double x = 0.2; using VecD = Chebyshev2::VectorDerivativeFunctor; VecD fx(M, N, x, 0, 3); - Matrix X = Matrix::Zero(M, N); - Matrix actualH(M, M * N); + Matrix X = Matrix::Zero(M, N + 1); + Matrix actualH(M, M * (N + 1)); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); // Test Jacobian - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -434,24 +435,24 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { const Vector points = Chebyshev2::Points(N, 0, T); // Assign the parameter matrix 1xN - Matrix X(1, N); - for (size_t i = 0; i < N; ++i) { + Matrix X(1, N + 1); + for (size_t i = 0; i <= N; ++i) { X(i) = f(points(i)); } // Evaluate the derivative at the chebyshev points using // VectorDerivativeFunctor. - for (size_t i = 0; i < N; ++i) { + for (size_t i = 0; i <= N; ++i) { VecD d(M, N, points(i), 0, T); Vector1 Dx = d(X); EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); } // Test Jacobian at the first chebyshev point. - Matrix actualH(M, M * N); + Matrix actualH(M, M * (N + 1)); VecD vecd(M, N, points(0), 0, T); vecd(X, actualH); - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -464,11 +465,11 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { using CompFunc = Chebyshev2::ComponentDerivativeFunctor; size_t row = 1; CompFunc fx(M, N, row, x, 0, 3); - Matrix X = Matrix::Zero(M, N); - Matrix actualH(1, M * N); + Matrix X = Matrix::Zero(M, (N + 1)); + Matrix actualH(1, M * (N + 1)); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -476,7 +477,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { //****************************************************************************** TEST(Chebyshev2, IntegralWeights) { const size_t N7 = 7; - Vector actual = Chebyshev2::IntegrationWeights(N7); + Vector actual = Chebyshev2::IntegrationWeights(N7 - 1); Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, 0.457142857142857, 0.520634920634921, 0.457142857142857, 0.253968253968254, 0.0285714285714286) @@ -484,7 +485,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); Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, 0.352242423718159, 0.437208405798326, 0.437208405798326, 0.352242423718159, 0.190141007218208, 0.0204081632653061)