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 using namespace std;
26 using namespace gtsam;
27 
28 auto model = noiseModel::Unit::Create(1);
29 
30 // Coefficients for testing, respectively 3 and 7 parameter Fourier basis.
31 // They correspond to best approximation of TestFunction(x)
32 const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished();
33 const Vector7 k7Coefficients =
34  (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943)
35  .finished();
36 
37 // The test-function used below
38 static double TestFunction(double x) { return exp(sin(x) + cos(x)); }
39 
40 //******************************************************************************
41 TEST(Basis, BasisEvaluationFunctor) {
42  // fx(0) takes coefficients c to calculate the value f(c;x==0)
43  FourierBasis::EvaluationFunctor fx(3, 0);
45  fx(k3Coefficients), 1e-9);
46 }
47 
48 //******************************************************************************
49 TEST(Basis, BasisEvaluationFunctorDerivative) {
50  // If we give the H argument, we get the derivative of fx(0) wrpt coefficients
51  // Needs to be Matrix so it can be used by OptionalJacobian.
52  Matrix H(1, 3);
53  FourierBasis::EvaluationFunctor fx(3, 0);
55  fx(k3Coefficients, H), 1e-9);
56 
57  Matrix13 expectedH(1, 1, 0);
58  EXPECT(assert_equal(expectedH, H));
59 }
60 
61 //******************************************************************************
62 TEST(Basis, Manual) {
63  const size_t N = 3;
64 
65  // We will create a linear factor graph
67 
68  // We create an unknown Vector expression for the coefficients
69  Key key(1);
70 
71  // We will need values below to test the Jacobians
72  Values values;
73  values.insert<Vector>(key, Vector::Zero(N)); // value does not really matter
74 
75  // At 16 different samples points x, check Predict_ expression
76  for (size_t i = 0; i < 16; i++) {
77  const double x = i * M_PI / 8;
78  const double desiredValue = TestFunction(x);
79 
80  // Manual JacobianFactor
81  Matrix A(1, N);
82  A << 1, cos(x), sin(x);
83  Vector b(1);
84  b << desiredValue;
85  JacobianFactor linearFactor(key, A, b);
86  graph.add(linearFactor);
87 
88  // Create factor to predict value at x
89  EvaluationFactor<FourierBasis> predictFactor(key, desiredValue, model, N,
90  x);
91 
92  // Check expression Jacobians
93  EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9);
94 
95  auto linearizedFactor = predictFactor.linearize(values);
96  auto linearizedJacobianFactor =
97  std::dynamic_pointer_cast<JacobianFactor>(linearizedFactor);
98  CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor
99  EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9));
100  }
101 
102  // Solve linear graph
103  VectorValues actual = graph.optimize();
104  EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4));
105 }
106 
107 //******************************************************************************
109  // Check fitting a function with a 7-parameter Fourier basis
110 
111  // Create linear factor graph
113  Key key(1);
114  for (size_t i = 0; i < 16; i++) {
115  double x = i * M_PI / 8, desiredValue = TestFunction(x);
117  model, 7, x);
118  }
119 
120  // Solve FourierFactorGraph
121  Values values;
122  values.insert<Vector>(key, Vector::Zero(7));
124  VectorValues actual = lfg->optimize();
125 
126  EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4));
127 }
128 
129 //******************************************************************************
130 TEST(Basis, WeightMatrix) {
131  // The WeightMatrix creates an m*n matrix, where m is the number of sample
132  // points, and n is the number of parameters.
133  Matrix expected(2, 3);
134  expected.row(0) << 1, cos(1), sin(1);
135  expected.row(1) << 1, cos(2), sin(2);
136  Vector2 X(1, 2);
137  Matrix actual = FourierBasis::WeightMatrix(3, X);
138  EXPECT(assert_equal(expected, actual, 1e-9));
139 }
140 
141 //******************************************************************************
142 TEST(Basis, Decomposition) {
143  // Create example sequence
145  for (size_t i = 0; i < 16; i++) {
146  double x = i * M_PI / 8, y = TestFunction(x);
147  sequence[x] = y;
148  }
149 
150  // Do Fourier Decomposition
152 
153  // Check
155 }
156 
157 //******************************************************************************
158 // Check derivative in two different ways: numerical and using D on f
159 double proxy(double x) {
160  return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients);
161 }
162 
163 TEST(Basis, Derivative7) {
164  // Check Derivative evaluation at point x=0.2
165 
166  // Calculate expected values by numerical derivative of proxy.
167  const double x = 0.2;
168  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy, x);
169 
170  // Calculate derivatives at Chebyshev points using D7, interpolate
171  Matrix D7 = FourierBasis::DifferentiationMatrix(7);
172  Vector derivativeCoefficients = D7 * k7Coefficients;
173  FourierBasis::EvaluationFunctor fx(7, x);
174  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8);
175 
176  // Do directly
177  FourierBasis::DerivativeFunctor dfdx(7, x);
178  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8);
179 }
180 
181 //******************************************************************************
182 TEST(Basis, VecDerivativeFunctor) {
183  using DotShape = typename FourierBasis::VectorDerivativeFunctor;
184  const size_t N = 3;
185 
186  // MATLAB example, Dec 27 2019, commit 014eded5
187  double h = 2 * M_PI / 16;
188  Vector2 dotShape(0.5556, -0.8315); // at h/2
189  DotShape dotShapeFunction(2, N, h / 2);
190  Matrix theta = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
191  .finished()
192  .transpose();
193  EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
194 }
195 
196 //******************************************************************************
197 // Suppose we want to parameterize a periodic function with function values at
198 // specific times, rather than coefficients. Can we do it? This would be a
199 // generalization of the Fourier transform, and constitute a "pseudo-spectral"
200 // parameterization. One way to do this is to establish hard constraints that
201 // express the relationship between the new parameters and the coefficients.
202 // For example, below I'd like the parameters to be the function values at
203 // X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients.
204 // Because the values f(X) = at these points can be written as f(X) = W(X)*c,
205 // we can simply express the coefficents c as c=inv(W(X))*f, which is a
206 // generalized Fourier transform. That also means we can create factors with the
207 // unknown f-values, as done manually below.
208 TEST(Basis, PseudoSpectral) {
209  // We will create a linear factor graph
211 
212  const size_t N = 3;
213  const Key key(1);
214 
215  // The correct values at X = {0.1,0.2,0.3} are simply W*c
216  const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished();
217  const Matrix W = FourierBasis::WeightMatrix(N, X);
218  const Vector expected = W * k3Coefficients;
219 
220  // Check those values are indeed correct values of Fourier approximation
221  using Eval = FourierBasis::EvaluationFunctor;
222  EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9);
223  EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9);
224  EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9);
225 
226  // Calculate "inverse Fourier transform" matrix
227  const Matrix invW = W.inverse();
228 
229  // At 16 different samples points x, add a factor on fExpr
230  for (size_t i = 0; i < 16; i++) {
231  const double x = i * M_PI / 8;
232  const double desiredValue = TestFunction(x);
233 
234  // Manual JacobianFactor
235  Matrix A(1, 3);
236  A << 1, cos(x), sin(x);
237  Vector b(1);
238  b << desiredValue;
239  JacobianFactor linearFactor(key, A * invW, b);
240  graph.add(linearFactor);
241  }
242 
243  // Solve linear graph
244  VectorValues actual = graph.optimize();
245  EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4));
246 }
247 
248 //******************************************************************************
249 int main() {
250  TestResult tr;
251  return TestRegistry::runAllTests(tr);
252 }
253 //******************************************************************************
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:249
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:142
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:32
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
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:43
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:159
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
k7Coefficients
const Vector7 k7Coefficients
Definition: testFourier.cpp:33
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:150
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
TEST
TEST(Basis, BasisEvaluationFunctor)
Definition: testFourier.cpp:41
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:47
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:40
N
#define N
Definition: igam.h:9
TestFunction
static double TestFunction(double x)
Definition: testFourier.cpp:38
M_PI
#define M_PI
Definition: mconf.h:117
model
auto model
Definition: testFourier.cpp:28
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 Fri Nov 1 2024 03:39:57