Merge pull request #1660 from borglab/chebyshev-improvements

Chebyshev2 Improvements

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) {
+    void calculateJacobian() {
-      H_.setZero(1, M_ * N);
+      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) {
+    void calculateJacobian() {
-      H_.setZero(1, M_ * N);
+      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,

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

gtsam/basis/Chebyshev2.h

@@ -51,27 +51,30 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
   using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
 
-  /// 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
+    // 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
     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;
   }
 

python/gtsam/tests/test_basis.py

@@ -11,10 +11,9 @@ Author: Frank Dellaert & Gerry Chen (Python)
 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]