testNumericalDerivative.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 
21 using namespace std;
22 using namespace gtsam;
23 
24 /* ************************************************************************* */
25 double f(const Vector2& x) {
26  assert(x.size() == 2);
27  return sin(x(0)) + cos(x(1));
28 }
29 
30 /* ************************************************************************* */
31 //
32 TEST(testNumericalDerivative, numericalGradient) {
33  Vector2 x(1, 1.1);
34 
35  Vector expected(2);
36  expected << cos(x(0)), -sin(x(1));
37 
38  Vector actual = numericalGradient<Vector2>(f, x);
39 
40  EXPECT(assert_equal(expected, actual, 1e-5));
41 }
42 
43 /* ************************************************************************* */
44 TEST(testNumericalDerivative, numericalHessian) {
45  Vector2 x(1, 1.1);
46 
47  Matrix expected(2, 2);
48  expected << -sin(x(0)), 0.0, 0.0, -cos(x(1));
49 
50  Matrix actual = numericalHessian<Vector2>(f, x);
51 
52  EXPECT(assert_equal(expected, actual, 1e-5));
53 }
54 
55 /* ************************************************************************* */
56 double f2(const Vector2& x) {
57  assert(x.size() == 2);
58  return sin(x(0)) * cos(x(1));
59 }
60 
61 /* ************************************************************************* */
62 //
63 TEST(testNumericalDerivative, numericalHessian2) {
64  Vector2 v(0.5, 1.0);
65  Vector2 x(v);
66 
67  Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1))
68  * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1))).finished();
69 
70  Matrix actual = numericalHessian(f2, x);
71 
72  EXPECT(assert_equal(expected, actual, 1e-5));
73 }
74 
75 /* ************************************************************************* */
76 double f3(double x1, double x2) {
77  return sin(x1) * cos(x2);
78 }
79 
80 /* ************************************************************************* */
81 //
82 TEST(testNumericalDerivative, numericalHessian211) {
83  double x1 = 1, x2 = 5;
84 
85  Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2)).finished();
86  Matrix actual11 = numericalHessian211<double, double>(f3, x1, x2);
87  EXPECT(assert_equal(expected11, actual11, 1e-5));
88 
89  Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2)).finished();
90  Matrix actual12 = numericalHessian212<double, double>(f3, x1, x2);
91  EXPECT(assert_equal(expected12, actual12, 1e-5));
92 
93  Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2)).finished();
94  Matrix actual22 = numericalHessian222<double, double>(f3, x1, x2);
95  EXPECT(assert_equal(expected22, actual22, 1e-5));
96 }
97 
98 TEST(testNumericalDerivative, numericalHessian212) {
99  // TODO should implement test for all the variants of numerical Hessian, for mixed dimension types,
100  // like Point3 y = Project(Camera, Point3);
101  // I'm not sure how numericalHessian212 is different from 211 or 222 -Mike B.
102 }
103 
104 /* ************************************************************************* */
105 double f4(double x, double y, double z) {
106  return sin(x) * cos(y) * z * z;
107 }
108 
109 /* ************************************************************************* */
110 //
111 TEST(testNumericalDerivative, numericalHessian311) {
112  double x = 1, y = 2, z = 3;
113  Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z).finished();
114  Matrix actual11 = numericalHessian311<double, double, double>(f4, x, y, z);
115  EXPECT(assert_equal(expected11, actual11, 1e-5));
116 
117  Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z).finished();
118  Matrix actual12 = numericalHessian312<double, double, double>(f4, x, y, z);
119  EXPECT(assert_equal(expected12, actual12, 1e-5));
120 
121  Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z).finished();
122  Matrix actual13 = numericalHessian313<double, double, double>(f4, x, y, z);
123  EXPECT(assert_equal(expected13, actual13, 1e-5));
124 
125  Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z).finished();
126  Matrix actual22 = numericalHessian322<double, double, double>(f4, x, y, z);
127  EXPECT(assert_equal(expected22, actual22, 1e-5));
128 
129  Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z).finished();
130  Matrix actual23 = numericalHessian323<double, double, double>(f4, x, y, z);
131  EXPECT(assert_equal(expected23, actual23, 1e-5));
132 
133  Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2).finished();
134  Matrix actual33 = numericalHessian333<double, double, double>(f4, x, y, z);
135  EXPECT(assert_equal(expected33, actual33, 1e-5));
136 }
137 
138 /* ************************************************************************* */
139 Vector6 f6(const double x1, const double x2, const double x3, const double x4,
140  const double x5, const double x6) {
141  Vector6 result;
142  result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6);
143  return result;
144 }
145 
146 Vector g6(const double x1, const double x2, const double x3, const double x4,
147  const double x5, const double x6) {
148  Vector result(6);
149  result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6);
150  return result;
151 }
152 
153 /* ************************************************************************* */
154 //
155 TEST(testNumericalDerivative, numeriDerivative61) {
156  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
157 
158  Matrix expected61 = (Matrix(6, 1) << cos(x1), 0, 0, 0, 0, 0).finished();
159  Matrix61 actual61 = numericalDerivative61<Vector6, double, double,
160  double, double, double, double>(f6, x1, x2, x3, x4, x5, x6);
161 
162  EXPECT(assert_equal(expected61, actual61, 1e-5));
163 
164  Matrix expected61Dynamic = Matrix::Zero(6, 1);
165  expected61Dynamic(0, 0) = cos(x1);
166  Matrix actual61Dynamic =
167  numericalDerivative61<Vector, double, double, double, double, double,
168  double, 1>(g6, x1, x2, x3, x4, x5, x6);
169 
170  EXPECT(assert_equal(expected61Dynamic, actual61Dynamic, 1e-5));
171 }
172 
173 /* ************************************************************************* */
174 //
175 TEST(testNumericalDerivative, numeriDerivative62) {
176  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
177 
178  Matrix expected62 = (Matrix(6, 1) << 0, -sin(x2), 0, 0, 0, 0).finished();
179  Matrix61 actual62 = numericalDerivative62<Vector6, double, double, double,
180  double, double, double>(f6, x1, x2, x3, x4, x5, x6);
181 
182  EXPECT(assert_equal(expected62, actual62, 1e-5));
183 
184  Matrix expected62Dynamic = Matrix::Zero(6, 1);
185  expected62Dynamic(1, 0) = -sin(x2);
186  Matrix61 actual62Dynamic = numericalDerivative62<Vector, double, double,
187  double, double, double, double, 1>(f6, x1, x2, x3, x4, x5, x6);
188 
189  EXPECT(assert_equal(expected62Dynamic, actual62Dynamic, 1e-5));
190 }
191 
192 /* ************************************************************************* */
193 //
194 TEST(testNumericalDerivative, numeriDerivative63) {
195  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
196 
197  Matrix expected63 = (Matrix(6, 1) << 0, 0, 2 * x3, 0, 0, 0).finished();
198  Matrix61 actual63 = numericalDerivative63<Vector6, double, double, double,
199  double, double, double>(f6, x1, x2, x3, x4, x5, x6);
200 
201  EXPECT(assert_equal(expected63, actual63, 1e-5));
202 
203  Matrix expected63Dynamic = Matrix::Zero(6, 1);
204  expected63Dynamic(2, 0) = 2 * x3;
205  Matrix61 actual63Dynamic =
206  numericalDerivative63<Vector, double, double, double, double, double,
207  double, 1>(f6, x1, x2, x3, x4, x5, x6);
208 
209  EXPECT(assert_equal(expected63Dynamic, actual63Dynamic, 1e-5));
210 }
211 
212 /* ************************************************************************* */
213 //
214 TEST(testNumericalDerivative, numeriDerivative64) {
215  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
216 
217  Matrix expected64 = (Matrix(6, 1) << 0, 0, 0, 3 * x4 * x4, 0, 0).finished();
218  Matrix61 actual64 = numericalDerivative64<Vector6, double, double, double,
219  double, double, double>(f6, x1, x2, x3, x4, x5, x6);
220 
221  EXPECT(assert_equal(expected64, actual64, 1e-5));
222 
223  Matrix expected64Dynamic = Matrix::Zero(6, 1);
224  expected64Dynamic(3, 0) = 3 * x4 * x4;
225  Matrix61 actual64Dynamic =
226  numericalDerivative64<Vector, double, double, double, double, double,
227  double, 1>(f6, x1, x2, x3, x4, x5, x6);
228 
229  EXPECT(assert_equal(expected64Dynamic, actual64Dynamic, 1e-5));
230 }
231 
232 /* ************************************************************************* */
233 //
234 TEST(testNumericalDerivative, numeriDerivative65) {
235  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
236 
237  Matrix expected65 = (Matrix(6, 1) << 0, 0, 0, 0, 0.5 / sqrt(x5), 0).finished();
238  Matrix61 actual65 = numericalDerivative65<Vector6, double, double, double,
239  double, double, double>(f6, x1, x2, x3, x4, x5, x6);
240 
241  EXPECT(assert_equal(expected65, actual65, 1e-5));
242 
243  Matrix expected65Dynamic = Matrix::Zero(6, 1);
244  expected65Dynamic(4, 0) = 0.5 / sqrt(x5);
245  Matrix61 actual65Dynamic =
246  numericalDerivative65<Vector, double, double, double, double, double,
247  double, 1>(f6, x1, x2, x3, x4, x5, x6);
248 
249  EXPECT(assert_equal(expected65Dynamic, actual65Dynamic, 1e-5));
250 }
251 
252 /* ************************************************************************* */
253 //
254 TEST(testNumericalDerivative, numeriDerivative66) {
255  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
256 
257  Matrix expected66 = (Matrix(6, 1) << 0, 0, 0, 0, 0, cos(x6) + sin(x6)).finished();
258  Matrix61 actual66 = numericalDerivative66<Vector6, double, double, double,
259  double, double, double>(f6, x1, x2, x3, x4, x5, x6);
260 
261  EXPECT(assert_equal(expected66, actual66, 1e-5));
262 
263  Matrix expected66Dynamic = Matrix::Zero(6, 1);
264  expected66Dynamic(5, 0) = cos(x6) + sin(x6);
265  Matrix61 actual66Dynamic =
266  numericalDerivative66<Vector, double, double, double, double, double,
267  double, 1>(f6, x1, x2, x3, x4, x5, x6);
268 
269  EXPECT(assert_equal(expected66Dynamic, actual66Dynamic, 1e-5));
270 }
271 
272 /* ************************************************************************* */
273 int main() {
274  TestResult tr;
275  return TestRegistry::runAllTests(tr);
276 }
277 /* ************************************************************************* */
internal::FixedSizeMatrix< Y, X6 >::type numericalDerivative66(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Scalar * y
static int runAllTests(TestResult &result)
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative62(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Matrix expected
Definition: testMatrix.cpp:971
Vector6 f6(const double x1, const double x2, const double x3, const double x4, const double x5, const double x6)
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative64(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Vector g6(const double x1, const double x2, const double x3, const double x4, const double x5, const double x6)
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Definition: BFloat16.h:88
Some functions to compute numerical derivatives.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
TEST(testNumericalDerivative, numericalGradient)
double f2(const Vector2 &x)
static string x5("x5")
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative63(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian311(std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
double f(const Vector2 &x)
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian211(std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
internal::FixedSizeMatrix< X1, X2 >::type numericalHessian212(std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
internal::FixedSizeMatrix< X, X >::type numericalHessian(std::function< double(const X &)> f, const X &x, double delta=1e-5)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
traits
Definition: chartTesting.h:28
internal::FixedSizeMatrix< Y, X5 >::type numericalDerivative65(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Eigen::Matrix< double, N, 1 > numericalGradient(std::function< double(const X &)> h, const X &x, double delta=1e-5)
Numerically compute gradient of scalar function.
double f4(double x, double y, double z)
Pose3 x1
Definition: testPose3.cpp:663
double f3(double x1, double x2)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
static string x4("x4")
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
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative61(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:52