12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Save half the cos() evals for Points

release/4.3a0
Frank Dellaert 2025-03-22 15:49:46 -04:00
parent fae071d603
commit ac4959d1fe
2 changed files with 47 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/basis/Chebyshev2.cpp
View File

@ -21,12 +21,42 @@
namespace gtsam { namespace gtsam {
double Chebyshev2::Point(size_t N, int j, double a, double b) { double Chebyshev2::Point(size_t N, int j) {
if (N == 1) return 0.0;
assert(j >= 0 && size_t(j) < N); assert(j >= 0 && size_t(j) < N);
const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); const double dtheta = M_PI / (N - 1);
// We add -PI so that we get values ordered from -1 to +1 return -cos(dtheta * j);
// sin(-M_PI_2 + dtheta*j); also works }
return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
double Chebyshev2::Point(size_t N, int j, double a, double b) {
if (N == 1) return (a + b) / 2;
return a + (b - a) * (Point(N, j) + 1.0) / 2.0;
}
Vector Chebyshev2::Points(size_t N) {
Vector points(N);
if (N == 1) {
points(0) = 0.0;
return points;
}
size_t half = N / 2;
const double dtheta = M_PI / (N - 1);
for (size_t j = 0; j < half; ++j) {
double c = cos(j * dtheta);
points(j) = -c;
points(N - 1 - j) = c;
}
if (N % 2 == 1) {
points(half) = 0.0;
}
return points;
}
Vector Chebyshev2::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;
} }
Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {

27
gtsam/basis/Chebyshev2.h
View File

@ -51,9 +51,17 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>; using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>; using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
/**
* @brief Specific Chebyshev point, within [-1,1] interval.
*
* @param N The degree of the polynomial
* @param j The index of the Chebyshev point
* @return double
*/
static double Point(size_t N, int j);
/** /**
* @brief Specific Chebyshev point, within [a,b] interval. * @brief Specific Chebyshev point, within [a,b] interval.
* Default interval is [-1, 1]
* *
* @param N The degree of the polynomial * @param N The degree of the polynomial
* @param j The index of the Chebyshev point * @param j The index of the Chebyshev point
@ -61,24 +69,13 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
* @param b Upper bound of interval (default: 1) * @param b Upper bound of interval (default: 1)
* @return double * @return double
*/ */
static double Point(size_t N, int j, double a = -1, double b = 1); static double Point(size_t N, int j, double a, double b);
/// All Chebyshev points /// All Chebyshev points
static Vector Points(size_t N) { static Vector Points(size_t N);
Vector points(N);
for (size_t j = 0; j < N; j++) {
points(j) = Point(N, j);
}
return points;
}
/// All Chebyshev points, within [a,b] interval /// All Chebyshev points, within [a,b] interval
static Vector Points(size_t N, double a, double b) { 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;
}
/** /**
* 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-1 (N values)