Chebyshev2.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/basis/Chebyshev2.h>
20 #include <cassert>
21 
22 namespace gtsam {
23 
24 double Chebyshev2::Point(size_t N, int j, double a, double b) {
25  assert(j >= 0 && size_t(j) < N);
26  const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
27  // We add -PI so that we get values ordered from -1 to +1
28  // sin(-M_PI_2 + dtheta*j); also works
29  return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
30 }
31 
32 Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
33  // Allocate space for weights
34  Weights weights(N);
35 
36  // We start by getting distances from x to all Chebyshev points
37  // as well as getting smallest distance
38  Weights distances(N);
39 
40  for (size_t j = 0; j < N; j++) {
41  const double dj =
42  x - Point(N, j, a, b); // only thing that depends on [a,b]
43 
44  if (std::abs(dj) < 1e-12) {
45  // exceptional case: x coincides with a Chebyshev point
46  weights.setZero();
47  weights(j) = 1;
48  return weights;
49  }
50  distances(j) = dj;
51  }
52 
53  // Beginning of interval, j = 0, x(0) = a
54  weights(0) = 0.5 / distances(0);
55 
56  // All intermediate points j=1:N-2
57  double d = weights(0), s = -1; // changes sign s at every iteration
58  for (size_t j = 1; j < N - 1; j++, s = -s) {
59  weights(j) = s / distances(j);
60  d += weights(j);
61  }
62 
63  // End of interval, j = N-1, x(N-1) = b
64  weights(N - 1) = 0.5 * s / distances(N - 1);
65  d += weights(N - 1);
66 
67  // normalize
68  return weights / d;
69 }
70 
71 Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
72  // Allocate space for weights
73  Weights weightDerivatives(N);
74 
75  // toggle variable so we don't need to use `pow` for -1
76  double t = -1;
77 
78  // We start by getting distances from x to all Chebyshev points
79  // as well as getting smallest distance
80  Weights distances(N);
81 
82  for (size_t j = 0; j < N; j++) {
83  const double dj =
84  x - Point(N, j, a, b); // only thing that depends on [a,b]
85  if (std::abs(dj) < 1e-12) {
86  // exceptional case: x coincides with a Chebyshev point
87  weightDerivatives.setZero();
88  // compute the jth row of the differentiation matrix for this point
89  double cj = (j == 0 || j == N - 1) ? 2. : 1.;
90  for (size_t k = 0; k < N; k++) {
91  if (j == 0 && k == 0) {
92  // we reverse the sign since we order the cheb points from -1 to 1
93  weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0;
94  } else if (j == N - 1 && k == N - 1) {
95  // we reverse the sign since we order the cheb points from -1 to 1
96  weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0;
97  } else if (k == j) {
98  double xj = Point(N, j);
99  double xj2 = xj * xj;
100  weightDerivatives(k) = -0.5 * xj / (1 - xj2);
101  } else {
102  double xj = Point(N, j);
103  double xk = Point(N, k);
104  double ck = (k == 0 || k == N - 1) ? 2. : 1.;
105  t = ((j + k) % 2) == 0 ? 1 : -1;
106  weightDerivatives(k) = (cj / ck) * t / (xj - xk);
107  }
108  }
109  return 2 * weightDerivatives / (b - a);
110  }
111  distances(j) = dj;
112  }
113 
114  // This section of code computes the derivative of
115  // the Barycentric Interpolation weights formula by applying
116  // the chain rule on the original formula.
117 
118  // g and k are multiplier terms which represent the derivatives of
119  // the numerator and denominator
120  double g = 0, k = 0;
121  double w = 1;
122 
123  for (size_t j = 0; j < N; j++) {
124  if (j == 0 || j == N - 1) {
125  w = 0.5;
126  } else {
127  w = 1.0;
128  }
129 
130  t = (j % 2 == 0) ? 1 : -1;
131 
132  double c = t / distances(j);
133  g += w * c;
134  k += (w * c / distances(j));
135  }
136 
137  double s = 1; // changes sign s at every iteration
138  double g2 = g * g;
139 
140  for (size_t j = 0; j < N; j++, s = -s) {
141  // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1,
142  // x0 = 1.0
143  if (j == 0 || j == N - 1) {
144  w = 0.5;
145  } else {
146  // All intermediate points j=1:N-2
147  w = 1.0;
148  }
149  weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) -
150  (w * -s * k / (g2 * distances(j)));
151  }
152 
153  return weightDerivatives;
154 }
155 
157  double b) {
158  DiffMatrix D(N, N);
159  if (N == 1) {
160  D(0, 0) = 1;
161  return D;
162  }
163 
164  // toggle variable so we don't need to use `pow` for -1
165  double t = -1;
166 
167  for (size_t i = 0; i < N; i++) {
168  double xi = Point(N, i);
169  double ci = (i == 0 || i == N - 1) ? 2. : 1.;
170  for (size_t j = 0; j < N; j++) {
171  if (i == 0 && j == 0) {
172  // we reverse the sign since we order the cheb points from -1 to 1
173  D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0;
174  } else if (i == N - 1 && j == N - 1) {
175  // we reverse the sign since we order the cheb points from -1 to 1
176  D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0;
177  } else if (i == j) {
178  double xi2 = xi * xi;
179  D(i, j) = -xi / (2 * (1 - xi2));
180  } else {
181  double xj = Point(N, j);
182  double cj = (j == 0 || j == N - 1) ? 2. : 1.;
183  t = ((i + j) % 2) == 0 ? 1 : -1;
184  D(i, j) = (ci / cj) * t / (xi - xj);
185  }
186  }
187  }
188  // scale the matrix to the range
189  return D / ((b - a) / 2.0);
190 }
191 
192 Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
193  // Allocate space for weights
194  Weights weights(N);
195  size_t K = N - 1, // number of intervals between N points
196  K2 = K * K;
197  weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
198  weights(N - 1) = weights(0);
199 
200  size_t last_k = K / 2 + K % 2 - 1;
201 
202  for (size_t i = 1; i <= N - 2; ++i) {
203  double theta = i * M_PI / K;
204  weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;
205 
206  for (size_t k = 1; k <= last_k; ++k)
207  weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1);
208  weights(i) *= (b - a) / K;
209  }
210 
211  return weights;
212 }
213 
214 } // namespace gtsam
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
Chebyshev2.h
Pseudo-spectral parameterization for Chebyshev polynomials of the second kind.
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
K2
static const Cal3Bundler K2(625, 1e-3, 1e-3)
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Chebyshev2::DifferentiationMatrix
static DiffMatrix DifferentiationMatrix(size_t N, double a=-1, double b=1)
Definition: Chebyshev2.cpp:156
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::Chebyshev2::Point
static double Point(size_t N, int j, double a=-1, double b=1)
Specific Chebyshev point, within [a,b] interval. Default interval is [-1, 1].
Definition: Chebyshev2.cpp:24
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Chebyshev2::CalculateWeights
static Weights CalculateWeights(size_t N, double x, double a=-1, double b=1)
Definition: Chebyshev2.cpp:32
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::Chebyshev2::IntegrationWeights
static Weights IntegrationWeights(size_t N, double a=-1, double b=1)
Definition: Chebyshev2.cpp:192
Eigen::Matrix< double, 1, -1 >
abs
#define abs(x)
Definition: datatypes.h:17
N
#define N
Definition: igam.h:9
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:562
M_PI
#define M_PI
Definition: mconf.h:117
align_3::t
Point2 t(10, 10)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::Chebyshev2::DerivativeWeights
static Weights DerivativeWeights(size_t N, double x, double a=-1, double b=1)
Definition: Chebyshev2.cpp:71


gtsam
Author(s):
autogenerated on Sun Feb 16 2025 04:01:04