From b7660930228fd72844df602fda9e4617dba4a32b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Sep 2023 23:04:53 -0400 Subject: [PATCH 01/12] improve bounds checks in Chebyshev2 --- gtsam/basis/Chebyshev2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 44876b6e91..63fca64cc4 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -32,7 +32,7 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { const double dj = x - Point(N, j, a, b); // only thing that depends on [a,b] - if (std::abs(dj) < 1e-10) { + if (std::abs(dj) < 1e-12) { // exceptional case: x coincides with a Chebyshev point weights.setZero(); weights(j) = 1; @@ -73,7 +73,7 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double 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] - if (std::abs(dj) < 1e-10) { + 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 From 79272bf8a8a1c06eecfad902937d7ef8bb4b8d5d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Sep 2023 23:05:25 -0400 Subject: [PATCH 02/12] overload the Chebyshev2::Point method to reduce duplication --- gtsam/basis/Chebyshev2.h | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 4c3542d568..55bb394880 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -51,27 +51,30 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { using Parameters = Eigen::Matrix; using DiffMatrix = Eigen::Matrix; - /// Specific Chebyshev point - 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 + * @param a Lower bound of interval (default: -1) + * @param b Upper bound of interval (default: 1) + * @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); // We add -PI so that we get values ordered from -1 to +1 - // sin(- M_PI_2 + dtheta*j); also works - return cos(-M_PI + dtheta * j); - } - - /// Specific Chebyshev point, within [a,b] interval - static double Point(size_t N, int j, double a, double b) { - assert(j >= 0 && size_t(j) < N); - const double dtheta = M_PI / (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; } /// 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); + for (size_t j = 0; j < N; j++) { + points(j) = Point(N, j); + } return points; } From 3bff8ad3170a3433bb5af553dedf70f772609122 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Oct 2023 17:55:39 -0400 Subject: [PATCH 03/12] use weights_.size() instead of passing in N --- gtsam/basis/Basis.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 0b2b3606b3..1b1c44acc0 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -239,8 +239,8 @@ class Basis { * i.e., one row of the Kronecker product of weights_ with the * MxM identity matrix. See also VectorEvaluationFunctor. */ - void calculateJacobian(size_t N) { - H_.setZero(1, M_ * N); + void calculateJacobian() { + H_.setZero(1, M_ * EvaluationFunctor::weights_.size()); for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j); } @@ -252,14 +252,14 @@ class Basis { /// Construct with row index VectorComponentFunctor(size_t M, size_t N, size_t i, double x) : EvaluationFunctor(N, x), M_(M), rowIndex_(i) { - calculateJacobian(N); + calculateJacobian(); } /// Construct with row index and interval VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a, double b) : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) { - calculateJacobian(N); + calculateJacobian(); } /// Calculate component of component rowIndex_ of P @@ -460,8 +460,8 @@ class Basis { * i.e., one row of the Kronecker product of weights_ with the * MxM identity matrix. See also VectorDerivativeFunctor. */ - void calculateJacobian(size_t N) { - H_.setZero(1, M_ * N); + void calculateJacobian() { + H_.setZero(1, M_ * this->weights_.size()); for (int j = 0; j < this->weights_.size(); j++) H_(0, rowIndex_ + j * M_) = this->weights_(j); } @@ -473,14 +473,14 @@ class Basis { /// Construct with row index ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x) : DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) { - calculateJacobian(N); + calculateJacobian(); } /// Construct with row index and interval ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a, double b) : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) { - calculateJacobian(N); + calculateJacobian(); } /// Calculate derivative of component rowIndex_ of F double apply(const Matrix& P, From 9ee652c04a4b1d7236349d5b0b9763d69c8a3018 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Oct 2023 17:57:07 -0400 Subject: [PATCH 04/12] ParameterMatrix as a separate static method --- gtsam/basis/Chebyshev.h | 10 ++++++++++ gtsam/basis/Chebyshev2.h | 7 ++++++- gtsam/basis/FitBasis.h | 2 +- gtsam/basis/Fourier.h | 5 +++++ 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 1c16c47bf6..25bd696981 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -34,6 +34,11 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis { Parameters parameters_; + /// Return a zero initialized Parameter matrix. + static Parameters ParameterMatrix(size_t N) { + return Parameters::Zero(N); + } + /** * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) * @@ -80,6 +85,11 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis { struct GTSAM_EXPORT Chebyshev2Basis : Basis { using Parameters = Eigen::Matrix; + /// Return a zero initialized Parameter matrix. + static Parameters ParameterMatrix(size_t N) { + return Parameters::Zero(N); + } + /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). * diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 55bb394880..6cc18bccc1 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -54,7 +54,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { /** * @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 * @param a Lower bound of interval (default: -1) @@ -86,6 +86,11 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { return points; } + /// Return a zero initialized Parameter matrix. + static Parameters ParameterMatrix(size_t N) { + return Parameters::Zero(N + 1); + } + /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) * These weights implement barycentric interpolation at a specific x. diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index f5cb99bd7e..6e7e809c75 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -74,7 +74,7 @@ class FitBasis { const Sequence& sequence, const SharedNoiseModel& model, size_t N) { NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); Values values; - values.insert(0, Parameters::Zero(N)); + values.insert(0, Basis::ParameterMatrix(N)); GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); return gfg; } diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index eb259bd8a9..65d1415613 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -51,6 +51,11 @@ class FourierBasis : public Basis { return b; } + /// Return a zero initialized Parameter matrix. + static Parameters ParameterMatrix(size_t N) { + return Parameters::Zero(N); + } + /** * @brief Evaluate Real Fourier Weights of size N in interval [a, b], * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) From fe7d87735216b701a85af6b51a59cf4cdfef1b3d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Oct 2023 18:14:18 -0400 Subject: [PATCH 05/12] make Chebyshev nodes over N+1 points instead of N --- gtsam/basis/Chebyshev2.cpp | 76 ++++++++-------- gtsam/basis/Chebyshev2.h | 14 +-- gtsam/basis/tests/testBasisFactors.cpp | 12 +-- gtsam/basis/tests/testChebyshev2.cpp | 119 +++++++++++++------------ 4 files changed, 111 insertions(+), 110 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 63fca64cc4..b8036b6352 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 6cc18bccc1..f7537eb64d 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 d33e13de6a..afe0168fe6 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 7807149364..9648ce86e7 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) From 36dc04d1260f20ad5c2f05d9032731803bdf2db3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Oct 2023 11:24:31 -0400 Subject: [PATCH 06/12] override WeightMatrix for Chebyshev2 --- gtsam/basis/Chebyshev2.cpp | 9 +++++++++ gtsam/basis/Chebyshev2.h | 10 ++++++++++ python/gtsam/tests/test_basis.py | 4 ++-- 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index b8036b6352..515ee3cce7 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -59,6 +59,15 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { return weights / d; } +Matrix Chebyshev2::WeightMatrix(size_t N, const Vector& X, double a, double b) { + // Chebyshev points go from 0 to N, hence N+1 points. + Matrix W(X.size(), N + 1); + for (int i = 0; i < X.size(); i++) { + W.row(i) = CalculateWeights(N, X(i), a, b); + } + return W; +} + Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weightDerivatives(N + 1); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index f7537eb64d..1f7e50a030 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -102,6 +102,16 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { static Weights CalculateWeights(size_t N, double x, double a = -1, double b = 1); + /** + * Calculate weights for all x in vector X. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis functions. + * + * Overriden for Chebyshev2. + */ + static Matrix WeightMatrix(size_t N, const Vector& X, double a = -1, + double b = 1); + /** * Evaluate derivative of barycentric weights. * This is easy and efficient via the DifferentiationMatrix. diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 5d3c5ace31..af423d10d4 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -11,10 +11,9 @@ import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam.utils.test_case import GtsamTestCase -from gtsam.symbol_shorthand import B class TestBasis(GtsamTestCase): @@ -26,6 +25,7 @@ class TestBasis(GtsamTestCase): Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is used. """ + def setUp(self): self.N = 2 self.x = [0., 0.5, 0.75] From 8312a71af4763ec84e0a5a5191da4a72f8f71232 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 6 Nov 2023 08:20:22 -0500 Subject: [PATCH 07/12] Revert "make Chebyshev nodes over N+1 points instead of N" This reverts commit fe7d87735216b701a85af6b51a59cf4cdfef1b3d. --- gtsam/basis/Chebyshev2.cpp | 76 ++++++++-------- gtsam/basis/Chebyshev2.h | 14 +-- gtsam/basis/tests/testBasisFactors.cpp | 12 +-- gtsam/basis/tests/testChebyshev2.cpp | 119 ++++++++++++------------- 4 files changed, 110 insertions(+), 111 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 515ee3cce7..5cfc380be8 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 + 1); + Weights weights(N); // We start by getting distances from x to all Chebyshev points - // as well as getting the smallest distance - Weights distances(N + 1); + // as well as getting smallest distance + Weights distances(N); - 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-1 + // All intermediate points j=1:N-2 double d = weights(0), s = -1; // changes sign s at every iteration - for (size_t j = 1; j < N; j++, s = -s) { + for (size_t j = 1; j < N - 1; j++, s = -s) { weights(j) = s / distances(j); d += weights(j); } - // End of interval, j = N, x(N) = b - weights(N) = 0.5 * s / distances(N); - d += weights(N); + // End of interval, j = N-1, x(N-1) = b + weights(N - 1) = 0.5 * s / distances(N - 1); + d += weights(N - 1); // normalize return weights / d; @@ -70,30 +70,30 @@ Matrix Chebyshev2::WeightMatrix(size_t N, const Vector& X, double a, double b) { Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { // Allocate space for weights - Weights weightDerivatives(N + 1); + 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 + 1); + Weights distances(N); - 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) ? 2. : 1.; - for (size_t k = 0; k <= N; k++) { + 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 * N + 1) / 6.0; - } else if (j == N && k == N) { + 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 * N + 1) / 6.0; + weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; } else if (k == j) { double xj = Point(N, j); double xj2 = xj * xj; @@ -101,7 +101,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) ? 2. : 1.; + double ck = (k == 0 || k == N - 1) ? 2. : 1.; t = ((j + k) % 2) == 0 ? 1 : -1; weightDerivatives(k) = (cj / ck) * t / (xj - xk); } @@ -120,8 +120,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) { + for (size_t j = 0; j < N; j++) { + if (j == 0 || j == N - 1) { w = 0.5; } else { w = 1.0; @@ -137,13 +137,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, x0 = 1.0 - if (j == 0 || j == N) { + 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) { w = 0.5; } else { - // All intermediate points j=1:N-1 + // All intermediate points j=1:N-2 w = 1.0; } weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - @@ -155,8 +155,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 + 1, N + 1); - if (N + 1 == 1) { + DiffMatrix D(N, N); + if (N == 1) { D(0, 0) = 1; return D; } @@ -164,22 +164,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) ? 2. : 1.; - for (size_t j = 0; j <= N; j++) { + 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 * N + 1) / 6.0; - } else if (i == N && j == N) { + 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 * N + 1) / 6.0; + 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) ? 2. : 1.; + double cj = (j == 0 || j == N - 1) ? 2. : 1.; t = ((i + j) % 2) == 0 ? 1 : -1; D(i, j) = (ci / cj) * t / (xi - xj); } @@ -191,15 +191,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 + 1); - size_t K = N, // number of intervals between N points + 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) = weights(0); + weights(N - 1) = weights(0); size_t last_k = K / 2 + K % 2 - 1; - for (size_t i = 1; i <= N - 1; ++i) { + 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; diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 1f7e50a030..25974adfd0 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); + 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; @@ -71,8 +71,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { /// All Chebyshev points static Vector Points(size_t N) { - Vector points(N + 1); - for (size_t j = 0; j <= N; j++) { + Vector points(N); + 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 (N+1 values) + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N 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 @@ -152,8 +152,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 + 1); - for (size_t j = 0; j <= N; j++) { + Matrix Xmat(M, N); + 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 afe0168fe6..d33e13de6a 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 + 1); + Vector functionValues(N); functionValues.setZero(); Values initial; @@ -84,7 +84,7 @@ TEST(BasisFactors, VectorEvaluationFactor) { NonlinearFactorGraph graph; graph.add(factor); - gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N + 1); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); 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 + 1); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N); 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 + 1); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N); 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 + 1); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); 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 + 1); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 9648ce86e7..7807149364 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 = 4; + static const int N = 5; auto points = Chebyshev2::Points(N); - Vector expected(N + 1); + Vector expected(N); 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 = 4; + static const int N = 5; auto points = Chebyshev2::Points(N, 0, 20); - Vector expected(N + 1); + Vector expected(N); 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 = 2; + size_t N = 3; Chebyshev2::EvaluationFunctor fx(N, 0.5); - Vector f(N + 1); + Vector f(N); 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(2, 1.5, 0, 2); + Chebyshev2::EvaluationFunctor fx(3, 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(4, 0.5); + Chebyshev2::EvaluationFunctor fx(5, 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 + 1); + Matrix X = Matrix::Zero(2, N); 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 + 1)); + Eigen::Matrix actualH(2, 2 * N); Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); @@ -123,7 +123,8 @@ 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)); } @@ -132,10 +133,10 @@ TEST(Chebyshev2, InterpolateVector) { TEST(Chebyshev2, InterpolatePose2) { double t = 30, a = 0, b = 100; - Matrix X(3, N + 1); + Matrix X(3, N); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp - X.row(1) = Vector::Zero(N + 1); - X.row(2) = 0.1 * Vector::Ones(N + 1); + X.row(1) = Vector::Zero(N); + X.row(2) = 0.1 * Vector::Ones(N); Vector xi(3); xi << t, 0, 0.1; @@ -150,7 +151,8 @@ 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)); } @@ -165,9 +167,9 @@ TEST(Chebyshev2, InterpolatePose3) { Pose3 pose(R, Point3(1, 2, 3)); Vector6 xi = Pose3::ChartAtOrigin::Local(pose); - Eigen::Matrix actualH(6, 6 * N + 1); + Eigen::Matrix actualH(6, 6 * N); - Matrix X = Matrix::Zero(6, N + 1); + Matrix X = Matrix::Zero(6, N); X.col(11) = xi; Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); @@ -179,7 +181,8 @@ 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 @@ -197,16 +200,16 @@ TEST(Chebyshev2, Decomposition) { FitBasis actual(sequence, model, 3); // Check - Vector expected(4); - expected << -1, -0.5, 0.5, 1; + Vector expected(3); + expected << -1, 0, 1; EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); } //****************************************************************************** TEST(Chebyshev2, DifferentiationMatrix3) { // Trefethen00book, p.55 - const size_t N = 2; - Matrix expected(N + 1, N + 1); + const size_t N = 3; + Matrix expected(N, N); // Differentiation matrix computed from chebfun expected << 1.5000, -2.0000, 0.5000, // 0.5000, -0.0000, -0.5000, // @@ -222,8 +225,8 @@ TEST(Chebyshev2, DifferentiationMatrix3) { //****************************************************************************** TEST(Chebyshev2, DerivativeMatrix6) { // Trefethen00book, p.55 - const size_t N = 5; - Matrix expected(N + 1, N + 1); + 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, // @@ -235,7 +238,7 @@ TEST(Chebyshev2, DerivativeMatrix6) { expected = -expected; Matrix actual = Chebyshev2::DifferentiationMatrix(N); - EXPECT(assert_equal(expected, actual, 1e-4)); + EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); } // test function for CalculateWeights and DerivativeWeights @@ -252,8 +255,8 @@ double fprime(double x) { //****************************************************************************** TEST(Chebyshev2, CalculateWeights) { - Eigen::Matrix fvals(N + 1); - for (size_t i = 0; i <= N; i++) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { fvals(i) = f(Chebyshev2::Point(N, i)); } double x1 = 0.7, x2 = -0.376; @@ -266,8 +269,8 @@ TEST(Chebyshev2, CalculateWeights) { TEST(Chebyshev2, CalculateWeights2) { double a = 0, b = 10, x1 = 7, x2 = 4.12; - Eigen::Matrix fvals(N + 1); - for (size_t i = 0; i <= N; i++) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { fvals(i) = f(Chebyshev2::Point(N, i, a, b)); } @@ -281,8 +284,8 @@ TEST(Chebyshev2, CalculateWeights2) { } TEST(Chebyshev2, DerivativeWeights) { - Eigen::Matrix fvals(N + 1); - for (size_t i = 0; i <= N; i++) { + Eigen::Matrix fvals(N); + 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; @@ -304,8 +307,8 @@ TEST(Chebyshev2, DerivativeWeights) { TEST(Chebyshev2, DerivativeWeights2) { double x1 = 5, x2 = 4.12, a = 0, b = 10; - Eigen::Matrix fvals(N + 1); - for (size_t i = 0; i <= N; i++) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { fvals(i) = f(Chebyshev2::Point(N, i, a, b)); } @@ -344,7 +347,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 + 1), Vector(D6 * x))); + EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); } //****************************************************************************** @@ -353,14 +356,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 + 1), Vector(D7 * x))); + EXPECT(assert_equal(Vector::Ones(N7), 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(5, x)(f3_at_6points); + return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); } TEST(Chebyshev2, Derivative6) { @@ -370,23 +373,21 @@ 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(N); + Matrix D6 = Chebyshev2::DifferentiationMatrix(6); Vector derivative_at_points = D6 * f3_at_6points; - Chebyshev2::EvaluationFunctor fx(N, x); + Chebyshev2::EvaluationFunctor fx(6, x); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); // Do directly - Chebyshev2::DerivativeFunctor dfdx(N, x); + Chebyshev2::DerivativeFunctor dfdx(6, 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(5, x, 0, 3)(f3_at_6points); + return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); } TEST(Chebyshev2, Derivative6_03) { @@ -396,16 +397,14 @@ 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(N, 0, 3); + Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); Vector derivative_at_points = D6 * f3_at_6points; - Chebyshev2::EvaluationFunctor fx(N, x, 0, 3); + Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); // Do directly - Chebyshev2::DerivativeFunctor dfdx(N, x, 0, 3); + Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); } @@ -416,12 +415,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 + 1); - Matrix actualH(M, M * (N + 1)); + Matrix X = Matrix::Zero(M, N); + Matrix actualH(M, M * N); 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)); } @@ -435,24 +434,24 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { const Vector points = Chebyshev2::Points(N, 0, T); // Assign the parameter matrix 1xN - Matrix X(1, N + 1); - for (size_t i = 0; i <= N; ++i) { + Matrix X(1, N); + 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 + 1)); + Matrix actualH(M, M * N); 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)); } @@ -465,11 +464,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 + 1)); - Matrix actualH(1, M * (N + 1)); + Matrix X = Matrix::Zero(M, N); + Matrix actualH(1, M * N); 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)); } @@ -477,7 +476,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { //****************************************************************************** TEST(Chebyshev2, IntegralWeights) { const size_t N7 = 7; - Vector actual = Chebyshev2::IntegrationWeights(N7 - 1); + Vector actual = Chebyshev2::IntegrationWeights(N7); Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, 0.457142857142857, 0.520634920634921, 0.457142857142857, 0.253968253968254, 0.0285714285714286) @@ -485,7 +484,7 @@ TEST(Chebyshev2, IntegralWeights) { EXPECT(assert_equal(expected, actual)); const size_t N8 = 8; - Vector actual2 = Chebyshev2::IntegrationWeights(N8 - 1); + Vector actual2 = Chebyshev2::IntegrationWeights(N8); Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, 0.352242423718159, 0.437208405798326, 0.437208405798326, 0.352242423718159, 0.190141007218208, 0.0204081632653061) From fb4957998f4856c9814bb478d4d121f05b82c279 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 6 Nov 2023 08:21:35 -0500 Subject: [PATCH 08/12] Chebyshev2::WeightMatrix to take only N points --- gtsam/basis/Chebyshev2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 5cfc380be8..c37fa9f6b7 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -61,7 +61,7 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { Matrix Chebyshev2::WeightMatrix(size_t N, const Vector& X, double a, double b) { // Chebyshev points go from 0 to N, hence N+1 points. - Matrix W(X.size(), N + 1); + Matrix W(X.size(), N); for (int i = 0; i < X.size(); i++) { W.row(i) = CalculateWeights(N, X(i), a, b); } From 10f75a02b2596655b592724d34658b3b4aa647d1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 6 Nov 2023 08:28:12 -0500 Subject: [PATCH 09/12] fix Chebyshev2 ParameterMatrix to take N values --- gtsam/basis/Chebyshev2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 25974adfd0..0c253fb75e 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -88,7 +88,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { /// Return a zero initialized Parameter matrix. static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N + 1); + return Parameters::Zero(N); } /** From e56e9b5ef91b700d183f3938eb07da90e57bcf38 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 6 Nov 2023 11:05:50 -0500 Subject: [PATCH 10/12] fix unittest assertion deprecation --- python/gtsam/tests/test_Robust.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Robust.py b/python/gtsam/tests/test_Robust.py index c2ab9d4a47..1d7e9f52d4 100644 --- a/python/gtsam/tests/test_Robust.py +++ b/python/gtsam/tests/test_Robust.py @@ -10,10 +10,11 @@ """ import unittest -import gtsam import numpy as np from gtsam.utils.test_case import GtsamTestCase +import gtsam + class TestRobust(GtsamTestCase): @@ -37,7 +38,7 @@ def custom_loss(e): v = gtsam.Values() v.insert(0, 0.0) - self.assertAlmostEquals(f.error(v), 0.125) + self.assertAlmostEqual(f.error(v), 0.125) if __name__ == "__main__": unittest.main() From bc1b949f8b616d218b7c121105a33734cd098da3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Dec 2023 14:10:29 -0500 Subject: [PATCH 11/12] remove ParameterMatrix method --- gtsam/basis/Chebyshev.h | 10 ---------- gtsam/basis/Chebyshev2.h | 5 ----- gtsam/basis/FitBasis.h | 2 +- gtsam/basis/Fourier.h | 5 ----- 4 files changed, 1 insertion(+), 21 deletions(-) diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 25bd696981..1c16c47bf6 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -34,11 +34,6 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis { Parameters parameters_; - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) * @@ -85,11 +80,6 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis { struct GTSAM_EXPORT Chebyshev2Basis : Basis { using Parameters = Eigen::Matrix; - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). * diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 0c253fb75e..6d0bc7f6bd 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -86,11 +86,6 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { return points; } - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) * These weights implement barycentric interpolation at a specific x. diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index 6e7e809c75..f5cb99bd7e 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -74,7 +74,7 @@ class FitBasis { const Sequence& sequence, const SharedNoiseModel& model, size_t N) { NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); Values values; - values.insert(0, Basis::ParameterMatrix(N)); + values.insert(0, Parameters::Zero(N)); GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); return gfg; } diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index 65d1415613..eb259bd8a9 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -51,11 +51,6 @@ class FourierBasis : public Basis { return b; } - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * @brief Evaluate Real Fourier Weights of size N in interval [a, b], * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) From 7799fd5a6a8f5059e09cb4173fb542b03c56894e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Dec 2023 14:13:11 -0500 Subject: [PATCH 12/12] remove overridden WeightMatrix method --- gtsam/basis/Chebyshev2.cpp | 9 --------- gtsam/basis/Chebyshev2.h | 10 ---------- 2 files changed, 19 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index c37fa9f6b7..63fca64cc4 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -59,15 +59,6 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { return weights / d; } -Matrix Chebyshev2::WeightMatrix(size_t N, const Vector& X, double a, double b) { - // Chebyshev points go from 0 to N, hence N+1 points. - Matrix W(X.size(), N); - for (int i = 0; i < X.size(); i++) { - W.row(i) = CalculateWeights(N, X(i), a, b); - } - return W; -} - Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weightDerivatives(N); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 6d0bc7f6bd..849a511049 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -97,16 +97,6 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { static Weights CalculateWeights(size_t N, double x, double a = -1, double b = 1); - /** - * Calculate weights for all x in vector X. - * Returns M*N matrix where M is the size of the vector X, - * and N is the number of basis functions. - * - * Overriden for Chebyshev2. - */ - static Matrix WeightMatrix(size_t N, const Vector& X, double a = -1, - double b = 1); - /** * Evaluate derivative of barycentric weights. * This is easy and efficient via the DifferentiationMatrix.