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

gtsam
Author(s):
autogenerated on Thu Dec 19 2024 04:06:54