Chebyshev2.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/base/Manifold.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/basis/Basis.h>

namespace gtsam {

class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  using Base = Basis<Chebyshev2>;
  using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
  using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;

  static double Point(size_t N, int j) {
    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);
  }

  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;
  }

  static Vector Points(size_t N) {
    Vector points(N);
    for (size_t j = 0; j < N; j++) points(j) = Point(N, j);
    return points;
  }

  static Vector Points(size_t N, double a, double b) {
    Vector points = Points(N);
    const double T1 = (a + b) / 2, T2 = (b - a) / 2;
    points = T1 + (T2 * points).array();
    return points;
  }

  static Weights CalculateWeights(size_t N, double x, double a = -1,
                                  double b = 1);

  static Weights DerivativeWeights(size_t N, double x, double a = -1,
                                   double b = 1);

  static DiffMatrix DifferentiationMatrix(size_t N, double a = -1,
                                          double b = 1);

  static Weights IntegrationWeights(size_t N, double a = -1, double b = 1);

  template <size_t M>
  static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
                       size_t N, double a = -1, double b = 1) {
    Matrix Xmat(M, N);
    for (size_t j = 0; j < N; j++) {
      Xmat.col(j) = f(Point(N, j, a, b));
    }
    return Xmat;
  }
};  // \ Chebyshev2

}  // namespace gtsam