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 
18 #include <gtsam/base/numericalDerivative.h>
19 #include <CppUnitLite/TestHarness.h>
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 /* ************************************************************************* */
gtsam::numericalDerivative66
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)
Definition: numericalDerivative.h:828
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
y
Scalar * y
Definition: level1_cplx_impl.h:124
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::numericalDerivative62
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)
Definition: numericalDerivative.h:680
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
f6
Vector6 f6(const double x1, const double x2, const double x3, const double x4, const double x5, const double x6)
Definition: testNumericalDerivative.cpp:139
gtsam::numericalDerivative64
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)
Definition: numericalDerivative.h:754
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
g6
Vector g6(const double x1, const double x2, const double x3, const double x4, const double x5, const double x6)
Definition: testNumericalDerivative.cpp:146
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
std
Definition: BFloat16.h:88
numericalDerivative.h
Some functions to compute numerical derivatives.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
TEST
TEST(testNumericalDerivative, numericalGradient)
Definition: testNumericalDerivative.cpp:32
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
x5
static string x5("x5")
gtsam::numericalDerivative63
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)
Definition: numericalDerivative.h:717
gtsam::numericalHessian311
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)
Definition: numericalDerivative.h:972
f
double f(const Vector2 &x)
Definition: testNumericalDerivative.cpp:25
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::numericalHessian211
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian211(std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:920
gtsam::numericalHessian212
internal::FixedSizeMatrix< X1, X2 >::type numericalHessian212(std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:901
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::numericalHessian
internal::FixedSizeMatrix< X, X >::type numericalHessian(std::function< double(const X &)> f, const X &x, double delta=1e-5)
Definition: numericalDerivative.h:861
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam
traits
Definition: chartTesting.h:28
gtsam::numericalDerivative65
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)
Definition: numericalDerivative.h:791
gtsam::numericalGradient
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.
Definition: numericalDerivative.h:70
f4
double f4(double x, double y, double z)
Definition: testNumericalDerivative.cpp:105
x1
Pose3 x1
Definition: testPose3.cpp:663
f3
double f3(double x1, double x2)
Definition: testNumericalDerivative.cpp:76
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
x4
static string x4("x4")
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
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::numericalDerivative61
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)
Definition: numericalDerivative.h:643

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