test_numerical_derivative.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 Unit tests for IMU numerical_derivative module.
9 Author: Frank Dellaert & Joel Truher
10 """
11 # pylint: disable=invalid-name, no-name-in-module
12 
13 import unittest
14 import numpy as np
15 
16 from gtsam import Pose3, Rot3, Point3
17 from gtsam.utils.numerical_derivative import numericalDerivative11, numericalDerivative21, numericalDerivative22, numericalDerivative33
18 
19 
20 class TestNumericalDerivatives(unittest.TestCase):
22  # Test function of one variable
23  def h(x):
24  return x ** 2
25 
26  x = np.array([3.0])
27  # Analytical derivative: dh/dx = 2x
28  analytical_derivative = np.array([[2.0 * x[0]]])
29 
30  # Compute numerical derivative
31  numerical_derivative = numericalDerivative11(h, x)
32 
33  # Check if numerical derivative is close to analytical derivative
34  np.testing.assert_allclose(
35  numerical_derivative, analytical_derivative, rtol=1e-5
36  )
37 
39  # Test function of one vector variable
40  def h(x):
41  return x ** 2
42 
43  x = np.array([1.0, 2.0, 3.0])
44  # Analytical derivative: dh/dx = 2x
45  analytical_derivative = np.diag(2.0 * x)
46 
47  numerical_derivative = numericalDerivative11(h, x)
48 
49  np.testing.assert_allclose(
50  numerical_derivative, analytical_derivative, rtol=1e-5
51  )
52 
54  # Test function of two variables, derivative with respect to first variable
55  def h(x1, x2):
56  return x1 * np.sin(x2)
57 
58  x1 = np.array([2.0])
59  x2 = np.array([np.pi / 4])
60  # Analytical derivative: dh/dx1 = sin(x2)
61  analytical_derivative = np.array([[np.sin(x2[0])]])
62 
63  numerical_derivative = numericalDerivative21(h, x1, x2)
64 
65  np.testing.assert_allclose(
66  numerical_derivative, analytical_derivative, rtol=1e-5
67  )
68 
70  # Test function of two variables, derivative with respect to second variable
71  def h(x1, x2):
72  return x1 * np.sin(x2)
73 
74  x1 = np.array([2.0])
75  x2 = np.array([np.pi / 4])
76  # Analytical derivative: dh/dx2 = x1 * cos(x2)
77  analytical_derivative = np.array([[x1[0] * np.cos(x2[0])]])
78 
79  numerical_derivative = numericalDerivative22(h, x1, x2)
80 
81  np.testing.assert_allclose(
82  numerical_derivative, analytical_derivative, rtol=1e-5
83  )
84 
86  # Test function of three variables, derivative with respect to third variable
87  def h(x1, x2, x3):
88  return x1 * x2 + np.exp(x3)
89 
90  x1 = np.array([1.0])
91  x2 = np.array([2.0])
92  x3 = np.array([0.5])
93  # Analytical derivative: dh/dx3 = exp(x3)
94  analytical_derivative = np.array([[np.exp(x3[0])]])
95 
96  numerical_derivative = numericalDerivative33(h, x1, x2, x3)
97 
98  np.testing.assert_allclose(
99  numerical_derivative, analytical_derivative, rtol=1e-5
100  )
101 
103  # Test function with manifold and vector inputs
104 
105  def h(pose:Pose3, point:np.ndarray):
106  return pose.transformFrom(point)
107 
108  # Values from testPose3.cpp
109  P = Point3(0.2,0.7,-2)
110  R = Rot3.Rodrigues(0.3,0,0)
111  P2 = Point3(3.5,-8.2,4.2)
112  T = Pose3(R,P2)
113 
114  analytic_H1 = np.zeros((3,6), order='F', dtype=float)
115  analytic_H2 = np.zeros((3,3), order='F', dtype=float)
116  y = T.transformFrom(P, analytic_H1, analytic_H2)
117 
118  numerical_H1 = numericalDerivative21(h, T, P)
119  numerical_H2 = numericalDerivative22(h, T, P)
120 
121  np.testing.assert_allclose(numerical_H1, analytic_H1, rtol=1e-5)
122  np.testing.assert_allclose(numerical_H2, analytic_H2, rtol=1e-5)
123 
124 if __name__ == "__main__":
125  unittest.main()
test_numerical_derivative.TestNumericalDerivatives.test_numericalDerivative_with_pose
def test_numericalDerivative_with_pose(self)
Definition: test_numerical_derivative.py:102
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:292
test_numerical_derivative.TestNumericalDerivatives.test_numericalDerivative22
def test_numericalDerivative22(self)
Definition: test_numerical_derivative.py:69
test_numerical_derivative.TestNumericalDerivatives.test_numericalDerivative11_vector
def test_numericalDerivative11_vector(self)
Definition: test_numerical_derivative.py:38
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:110
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
h
const double h
Definition: testSimpleHelicopter.cpp:19
test_numerical_derivative.TestNumericalDerivatives.test_numericalDerivative33
def test_numericalDerivative33(self)
Definition: test_numerical_derivative.py:85
test_numerical_derivative.TestNumericalDerivatives.test_numericalDerivative21
def test_numericalDerivative21(self)
Definition: test_numerical_derivative.py:53
test_numerical_derivative.TestNumericalDerivatives
Definition: test_numerical_derivative.py:20
gtsam::Pose3
Definition: Pose3.h:37
gtsam::utils.numerical_derivative
Definition: numerical_derivative.py:1
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
test_numerical_derivative.TestNumericalDerivatives.test_numericalDerivative11_scalar
def test_numericalDerivative11_scalar(self)
Definition: test_numerical_derivative.py:21


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:05