2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Unit tests for IMU numerical_derivative module.
9 Author: Frank Dellaert & Joel Truher
16 from gtsam
import Pose3, Rot3, Point3
28 analytical_derivative = np.array([[2.0 * x[0]]])
34 np.testing.assert_allclose(
35 numerical_derivative, analytical_derivative, rtol=1e-5
43 x = np.array([1.0, 2.0, 3.0])
45 analytical_derivative = np.diag(2.0 * x)
49 np.testing.assert_allclose(
50 numerical_derivative, analytical_derivative, rtol=1e-5
56 return x1 * np.sin(x2)
59 x2 = np.array([np.pi / 4])
61 analytical_derivative = np.array([[np.sin(x2[0])]])
65 np.testing.assert_allclose(
66 numerical_derivative, analytical_derivative, rtol=1e-5
72 return x1 * np.sin(x2)
75 x2 = np.array([np.pi / 4])
77 analytical_derivative = np.array([[x1[0] * np.cos(x2[0])]])
81 np.testing.assert_allclose(
82 numerical_derivative, analytical_derivative, rtol=1e-5
88 return x1 * x2 + np.exp(x3)
94 analytical_derivative = np.array([[np.exp(x3[0])]])
98 np.testing.assert_allclose(
99 numerical_derivative, analytical_derivative, rtol=1e-5
105 def h(pose:Pose3, point:np.ndarray):
106 return pose.transformFrom(point)
110 R = Rot3.Rodrigues(0.3,0,0)
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)
121 np.testing.assert_allclose(numerical_H1, analytic_H1, rtol=1e-5)
122 np.testing.assert_allclose(numerical_H2, analytic_H2, rtol=1e-5)
124 if __name__ ==
"__main__":