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, 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 diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 4c3542d568..849a511049 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; } 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]