testFourier.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 
20 #include <gtsam/base/Testable.h>
21 #include <gtsam/basis/FitBasis.h>
22 #include <gtsam/basis/Fourier.h>
24 
25 #if defined(__GNUC__) && !defined(__clang__)
26 #pragma GCC diagnostic warning "-Wstringop-overread"
27 #pragma GCC diagnostic warning "-Warray-bounds"
28 #endif
29 using namespace std;
30 using namespace gtsam;
31 
32 auto model = noiseModel::Unit::Create(1);
33 
34 // Coefficients for testing, respectively 3 and 7 parameter Fourier basis.
35 // They correspond to best approximation of TestFunction(x)
36 const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished();
37 const Vector7 k7Coefficients =
38  (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943)
39  .finished();
40 
41 // The test-function used below
42 static double TestFunction(double x) { return exp(sin(x) + cos(x)); }
43 
44 //******************************************************************************
45 TEST(Basis, BasisEvaluationFunctor) {
46  // fx(0) takes coefficients c to calculate the value f(c;x==0)
47  FourierBasis::EvaluationFunctor fx(3, 0);
49  fx(k3Coefficients), 1e-9);
50 }
51 
52 //******************************************************************************
53 TEST(Basis, BasisEvaluationFunctorDerivative) {
54  // If we give the H argument, we get the derivative of fx(0) wrpt coefficients
55  // Needs to be Matrix so it can be used by OptionalJacobian.
56  Matrix H(1, 3);
57  FourierBasis::EvaluationFunctor fx(3, 0);
59  fx(k3Coefficients, H), 1e-9);
60 
61  Matrix13 expectedH(1, 1, 0);
62  EXPECT(assert_equal(expectedH, H));
63 }
64 
65 //******************************************************************************
66 TEST(Basis, Manual) {
67  const size_t N = 3;
68 
69  // We will create a linear factor graph
71 
72  // We create an unknown Vector expression for the coefficients
73  Key key(1);
74 
75  // We will need values below to test the Jacobians
76  Values values;
77  values.insert<Vector>(key, Vector::Zero(N)); // value does not really matter
78 
79  // At 16 different samples points x, check Predict_ expression
80  for (size_t i = 0; i < 16; i++) {
81  const double x = i * M_PI / 8;
82  const double desiredValue = TestFunction(x);
83 
84  // Manual JacobianFactor
85  Matrix A(1, N);
86  A << 1, cos(x), sin(x);
87  Vector b(1);
88  b << desiredValue;
89  JacobianFactor linearFactor(key, A, b);
90  graph.add(linearFactor);
91 
92  // Create factor to predict value at x
93  EvaluationFactor<FourierBasis> predictFactor(key, desiredValue, model, N,
94  x);
95 
96  // Check expression Jacobians
97  EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9);
98 
99  auto linearizedFactor = predictFactor.linearize(values);
100  auto linearizedJacobianFactor =
101  std::dynamic_pointer_cast<JacobianFactor>(linearizedFactor);
102  CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor
103  EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9));
104  }
105 
106  // Solve linear graph
107  VectorValues actual = graph.optimize();
108  EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4));
109 }
110 
111 //******************************************************************************
113  // Check fitting a function with a 7-parameter Fourier basis
114 
115  // Create linear factor graph
117  Key key(1);
118  for (size_t i = 0; i < 16; i++) {
119  double x = i * M_PI / 8, desiredValue = TestFunction(x);
121  model, 7, x);
122  }
123 
124  // Solve FourierFactorGraph
125  Values values;
126  values.insert<Vector>(key, Vector::Zero(7));
128  VectorValues actual = lfg->optimize();
129 
130  EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4));
131 }
132 
133 //******************************************************************************
134 TEST(Basis, WeightMatrix) {
135  // The WeightMatrix creates an m*n matrix, where m is the number of sample
136  // points, and n is the number of parameters.
137  Matrix expected(2, 3);
138  expected.row(0) << 1, cos(1), sin(1);
139  expected.row(1) << 1, cos(2), sin(2);
140  Vector2 X(1, 2);
141  Matrix actual = FourierBasis::WeightMatrix(3, X);
142  EXPECT(assert_equal(expected, actual, 1e-9));
143 }
144 
145 //******************************************************************************
146 TEST(Basis, Decomposition) {
147  // Create example sequence
149  for (size_t i = 0; i < 16; i++) {
150  double x = i * M_PI / 8, y = TestFunction(x);
151  sequence[x] = y;
152  }
153 
154  // Do Fourier Decomposition
156 
157  // Check
159 }
160 
161 //******************************************************************************
162 // Check derivative in two different ways: numerical and using D on f
163 double proxy(double x) {
164  return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients);
165 }
166 
167 TEST(Basis, Derivative7) {
168  // Check Derivative evaluation at point x=0.2
169 
170  // Calculate expected values by numerical derivative of proxy.
171  const double x = 0.2;
172  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy, x);
173 
174  // Calculate derivatives at Chebyshev points using D7, interpolate
175  Matrix D7 = FourierBasis::DifferentiationMatrix(7);
176  Vector derivativeCoefficients = D7 * k7Coefficients;
177  FourierBasis::EvaluationFunctor fx(7, x);
178  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8);
179 
180  // Do directly
181  FourierBasis::DerivativeFunctor dfdx(7, x);
182  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8);
183 }
184 
185 //******************************************************************************
186 TEST(Basis, VecDerivativeFunctor) {
187  using DotShape = typename FourierBasis::VectorDerivativeFunctor;
188  const size_t N = 3;
189 
190  // MATLAB example, Dec 27 2019, commit 014eded5
191  double h = 2 * M_PI / 16;
192  Vector2 dotShape(0.5556, -0.8315); // at h/2
193  DotShape dotShapeFunction(2, N, h / 2);
194  Matrix theta = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
195  .finished()
196  .transpose();
197  EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
198 }
199 
200 //******************************************************************************
201 // Suppose we want to parameterize a periodic function with function values at
202 // specific times, rather than coefficients. Can we do it? This would be a
203 // generalization of the Fourier transform, and constitute a "pseudo-spectral"
204 // parameterization. One way to do this is to establish hard constraints that
205 // express the relationship between the new parameters and the coefficients.
206 // For example, below I'd like the parameters to be the function values at
207 // X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients.
208 // Because the values f(X) = at these points can be written as f(X) = W(X)*c,
209 // we can simply express the coefficents c as c=inv(W(X))*f, which is a
210 // generalized Fourier transform. That also means we can create factors with the
211 // unknown f-values, as done manually below.
212 TEST(Basis, PseudoSpectral) {
213  // We will create a linear factor graph
215 
216  const size_t N = 3;
217  const Key key(1);
218 
219  // The correct values at X = {0.1,0.2,0.3} are simply W*c
220  const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished();
221  const Matrix W = FourierBasis::WeightMatrix(N, X);
222  const Vector expected = W * k3Coefficients;
223 
224  // Check those values are indeed correct values of Fourier approximation
225  using Eval = FourierBasis::EvaluationFunctor;
226  EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9);
227  EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9);
228  EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9);
229 
230  // Calculate "inverse Fourier transform" matrix
231  const Matrix invW = W.inverse();
232 
233  // At 16 different samples points x, add a factor on fExpr
234  for (size_t i = 0; i < 16; i++) {
235  const double x = i * M_PI / 8;
236  const double desiredValue = TestFunction(x);
237 
238  // Manual JacobianFactor
239  Matrix A(1, 3);
240  A << 1, cos(x), sin(x);
241  Vector b(1);
242  b << desiredValue;
243  JacobianFactor linearFactor(key, A * invW, b);
244  graph.add(linearFactor);
245  }
246 
247  // Solve linear graph
248  VectorValues actual = graph.optimize();
249  EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4));
250 }
251 
252 //******************************************************************************
253 int main() {
254  TestResult tr;
255  return TestRegistry::runAllTests(tr);
256 }
257 //******************************************************************************
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
H
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
EXPECT_CORRECT_FACTOR_JACOBIANS
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Definition: factorTesting.h:114
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
main
int main()
Definition: testFourier.cpp:253
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::VectorValues::at
Vector & at(Key j)
Definition: VectorValues.h:145
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
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::Basis
Definition: Basis.h:90
gtsam::EvaluationFactor
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition: BasisFactors.h:39
k3Coefficients
const Vector k3Coefficients
Definition: testFourier.cpp:36
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:247
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
X
#define X
Definition: icosphere.cpp:20
gtsam::FitBasis
Definition: FitBasis.h:52
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
h
const double h
Definition: testSimpleHelicopter.cpp:19
proxy
double proxy(double x)
Definition: testFourier.cpp:163
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
k7Coefficients
const Vector7 k7Coefficients
Definition: testFourier.cpp:37
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::NoiseModelFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: NonlinearFactor.cpp:152
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
TEST
TEST(Basis, BasisEvaluationFunctor)
Definition: testFourier.cpp:45
sequence
Definition: pytypes.h:2146
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::Sequence
std::map< double, double > Sequence
Our sequence representation is a map of {x: y} values where y = f(x)
Definition: FitBasis.h:36
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
switching3::lfg
const HybridGaussianFactorGraph & lfg
Definition: testHybridGaussianISAM.cpp:48
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
Vector2
Definition: test_operator_overloading.cpp:18
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
gtsam::symbol_shorthand::W
Key W(std::uint64_t j)
Definition: inference/Symbol.h:170
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
FitBasis.h
Fit a Basis using least-squares.
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
N
#define N
Definition: igam.h:9
TestFunction
static double TestFunction(double x)
Definition: testFourier.cpp:42
M_PI
#define M_PI
Definition: mconf.h:117
model
auto model
Definition: testFourier.cpp:32
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Fourier.h
Fourier decomposition, see e.g. http://mathworld.wolfram.com/FourierSeries.html.
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::FitBasis::parameters
Parameters parameters() const
Return Fourier coefficients.
Definition: FitBasis.h:96


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