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 
21 namespace gtsam {
22 
23 Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
24  // Allocate space for weights
25  Weights weights(N);
26 
27  // We start by getting distances from x to all Chebyshev points
28  // as well as getting smallest distance
29  Weights distances(N);
30 
31  for (size_t j = 0; j < N; j++) {
32  const double dj =
33  x - Point(N, j, a, b); // only thing that depends on [a,b]
34 
35  if (std::abs(dj) < 1e-12) {
36  // exceptional case: x coincides with a Chebyshev point
37  weights.setZero();
38  weights(j) = 1;
39  return weights;
40  }
41  distances(j) = dj;
42  }
43 
44  // Beginning of interval, j = 0, x(0) = a
45  weights(0) = 0.5 / distances(0);
46 
47  // All intermediate points j=1:N-2
48  double d = weights(0), s = -1; // changes sign s at every iteration
49  for (size_t j = 1; j < N - 1; j++, s = -s) {
50  weights(j) = s / distances(j);
51  d += weights(j);
52  }
53 
54  // End of interval, j = N-1, x(N-1) = b
55  weights(N - 1) = 0.5 * s / distances(N - 1);
56  d += weights(N - 1);
57 
58  // normalize
59  return weights / d;
60 }
61 
62 Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
63  // Allocate space for weights
64  Weights weightDerivatives(N);
65 
66  // toggle variable so we don't need to use `pow` for -1
67  double t = -1;
68 
69  // We start by getting distances from x to all Chebyshev points
70  // as well as getting smallest distance
71  Weights distances(N);
72 
73  for (size_t j = 0; j < N; j++) {
74  const double dj =
75  x - Point(N, j, a, b); // only thing that depends on [a,b]
76  if (std::abs(dj) < 1e-12) {
77  // exceptional case: x coincides with a Chebyshev point
78  weightDerivatives.setZero();
79  // compute the jth row of the differentiation matrix for this point
80  double cj = (j == 0 || j == N - 1) ? 2. : 1.;
81  for (size_t k = 0; k < N; k++) {
82  if (j == 0 && k == 0) {
83  // we reverse the sign since we order the cheb points from -1 to 1
84  weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0;
85  } else if (j == N - 1 && k == N - 1) {
86  // we reverse the sign since we order the cheb points from -1 to 1
87  weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0;
88  } else if (k == j) {
89  double xj = Point(N, j);
90  double xj2 = xj * xj;
91  weightDerivatives(k) = -0.5 * xj / (1 - xj2);
92  } else {
93  double xj = Point(N, j);
94  double xk = Point(N, k);
95  double ck = (k == 0 || k == N - 1) ? 2. : 1.;
96  t = ((j + k) % 2) == 0 ? 1 : -1;
97  weightDerivatives(k) = (cj / ck) * t / (xj - xk);
98  }
99  }
100  return 2 * weightDerivatives / (b - a);
101  }
102  distances(j) = dj;
103  }
104 
105  // This section of code computes the derivative of
106  // the Barycentric Interpolation weights formula by applying
107  // the chain rule on the original formula.
108 
109  // g and k are multiplier terms which represent the derivatives of
110  // the numerator and denominator
111  double g = 0, k = 0;
112  double w = 1;
113 
114  for (size_t j = 0; j < N; j++) {
115  if (j == 0 || j == N - 1) {
116  w = 0.5;
117  } else {
118  w = 1.0;
119  }
120 
121  t = (j % 2 == 0) ? 1 : -1;
122 
123  double c = t / distances(j);
124  g += w * c;
125  k += (w * c / distances(j));
126  }
127 
128  double s = 1; // changes sign s at every iteration
129  double g2 = g * g;
130 
131  for (size_t j = 0; j < N; j++, s = -s) {
132  // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1,
133  // x0 = 1.0
134  if (j == 0 || j == N - 1) {
135  w = 0.5;
136  } else {
137  // All intermediate points j=1:N-2
138  w = 1.0;
139  }
140  weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) -
141  (w * -s * k / (g2 * distances(j)));
142  }
143 
144  return weightDerivatives;
145 }
146 
148  double b) {
149  DiffMatrix D(N, N);
150  if (N == 1) {
151  D(0, 0) = 1;
152  return D;
153  }
154 
155  // toggle variable so we don't need to use `pow` for -1
156  double t = -1;
157 
158  for (size_t i = 0; i < N; i++) {
159  double xi = Point(N, i);
160  double ci = (i == 0 || i == N - 1) ? 2. : 1.;
161  for (size_t j = 0; j < N; j++) {
162  if (i == 0 && j == 0) {
163  // we reverse the sign since we order the cheb points from -1 to 1
164  D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0;
165  } else if (i == N - 1 && j == N - 1) {
166  // we reverse the sign since we order the cheb points from -1 to 1
167  D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0;
168  } else if (i == j) {
169  double xi2 = xi * xi;
170  D(i, j) = -xi / (2 * (1 - xi2));
171  } else {
172  double xj = Point(N, j);
173  double cj = (j == 0 || j == N - 1) ? 2. : 1.;
174  t = ((i + j) % 2) == 0 ? 1 : -1;
175  D(i, j) = (ci / cj) * t / (xi - xj);
176  }
177  }
178  }
179  // scale the matrix to the range
180  return D / ((b - a) / 2.0);
181 }
182 
183 Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
184  // Allocate space for weights
185  Weights weights(N);
186  size_t K = N - 1, // number of intervals between N points
187  K2 = K * K;
188  weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
189  weights(N - 1) = weights(0);
190 
191  size_t last_k = K / 2 + K % 2 - 1;
192 
193  for (size_t i = 1; i <= N - 2; ++i) {
194  double theta = i * M_PI / K;
195  weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;
196 
197  for (size_t k = 1; k <= last_k; ++k)
198  weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1);
199  weights(i) *= (b - a) / K;
200  }
201 
202  return weights;
203 }
204 
205 } // 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
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.h:64
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:147
g2
Pose3 g2(g1.expmap(h *V1_g1))
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:23
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:183
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:62


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:05