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 Numerical derivative functions.
9 Author: Joel Truher & Frank Dellaert
10 """
11 
12 # pylint: disable=C0103,C0114,C0116,E0611,R0913
13 # mypy: disable-error-code="import-untyped"
14 # see numericalDerivative.h
15 
16 # pybind wants to wrap concrete types, which would have been
17 # a whole lot of them, so i reimplemented the part of this that
18 # I needed, using the python approach to "generic" typing.
19 
20 from typing import Callable, TypeVar
21 import numpy as np
22 
23 Y = TypeVar("Y")
24 X = TypeVar("X")
25 X1 = TypeVar("X1")
26 X2 = TypeVar("X2")
27 X3 = TypeVar("X3")
28 X4 = TypeVar("X4")
29 X5 = TypeVar("X5")
30 X6 = TypeVar("X6")
31 
32 
33 def local(a: Y, b: Y) -> np.ndarray:
34  if type(a) is not type(b):
35  raise TypeError(f"a {type(a)} b {type(b)}")
36  if isinstance(a, np.ndarray):
37  return b - a
38  if isinstance(a, (float, int)):
39  return np.ndarray([[b - a]]) # type:ignore
40  # there is no common superclass for Y
41  return a.localCoordinates(b) # type:ignore
42 
43 
44 def retract(a, xi: np.ndarray):
45  if isinstance(a, (np.ndarray, float, int)):
46  return a + xi
47  return a.retract(xi)
48 
49 
50 def numericalDerivative11(h: Callable[[X], Y], x: X, delta=1e-5) -> np.ndarray:
51  hx: Y = h(x)
52  zeroY = local(hx, hx)
53  m = zeroY.shape[0]
54  zeroX = local(x, x)
55  N = zeroX.shape[0]
56  dx = np.zeros(N)
57  H = np.zeros((m, N))
58  factor: float = 1.0 / (2.0 * delta)
59  for j in range(N):
60  dx[j] = delta
61  dy1 = local(hx, h(retract(x, dx)))
62  dx[j] = -delta
63  dy2 = local(hx, h(retract(x, dx)))
64  dx[j] = 0
65  H[:, j] = (dy1 - dy2) * factor
66  return H
67 
68 
70  h: Callable[[X1, X2], Y], x1: X1, x2: X2, delta=1e-5
71 ) -> np.ndarray:
72  return numericalDerivative11(lambda x: h(x, x2), x1, delta)
73 
74 
76  h: Callable[[X1, X2], Y], x1: X1, x2: X2, delta=1e-5
77 ) -> np.ndarray:
78  return numericalDerivative11(lambda x: h(x1, x), x2, delta)
79 
80 
82  h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5
83 ) -> np.ndarray:
84  return numericalDerivative11(lambda x: h(x, x2, x3), x1, delta)
85 
86 
88  h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5
89 ) -> np.ndarray:
90  return numericalDerivative11(lambda x: h(x1, x, x3), x2, delta)
91 
92 
94  h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5
95 ) -> np.ndarray:
96  return numericalDerivative11(lambda x: h(x1, x2, x), x3, delta)
97 
98 
100  h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
101 ) -> np.ndarray:
102  return numericalDerivative11(lambda x: h(x, x2, x3, x4), x1, delta)
103 
104 
106  h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
107 ) -> np.ndarray:
108  return numericalDerivative11(lambda x: h(x1, x, x3, x4), x2, delta)
109 
110 
112  h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
113 ) -> np.ndarray:
114  return numericalDerivative11(lambda x: h(x1, x2, x, x4), x3, delta)
115 
116 
118  h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
119 ) -> np.ndarray:
120  return numericalDerivative11(lambda x: h(x1, x2, x3, x), x4, delta)
121 
122 
124  h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
125 ) -> np.ndarray:
126  return numericalDerivative11(lambda x: h(x, x2, x3, x4, x5), x1, delta)
127 
128 
130  h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
131 ) -> np.ndarray:
132  return numericalDerivative11(lambda x: h(x1, x, x3, x4, x5), x2, delta)
133 
134 
136  h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
137 ) -> np.ndarray:
138  return numericalDerivative11(lambda x: h(x1, x2, x, x4, x5), x3, delta)
139 
140 
142  h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
143 ) -> np.ndarray:
144  return numericalDerivative11(lambda x: h(x1, x2, x3, x, x5), x4, delta)
145 
146 
148  h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
149 ) -> np.ndarray:
150  return numericalDerivative11(lambda x: h(x1, x2, x3, x4, x), x5, delta)
151 
152 
154  h: Callable[[X1, X2, X3, X4, X5, X6], Y],
155  x1: X1,
156  x2: X2,
157  x3: X3,
158  x4: X4,
159  x5: X5,
160  x6: X6,
161  delta=1e-5,
162 ) -> np.ndarray:
163  return numericalDerivative11(lambda x: h(x, x2, x3, x4, x5, x6), x1, delta)
164 
165 
167  h: Callable[[X1, X2, X3, X4, X5, X6], Y],
168  x1: X1,
169  x2: X2,
170  x3: X3,
171  x4: X4,
172  x5: X5,
173  x6: X6,
174  delta=1e-5,
175 ) -> np.ndarray:
176  return numericalDerivative11(lambda x: h(x1, x, x3, x4, x5, x6), x2, delta)
177 
178 
180  h: Callable[[X1, X2, X3, X4, X5, X6], Y],
181  x1: X1,
182  x2: X2,
183  x3: X3,
184  x4: X4,
185  x5: X5,
186  x6: X6,
187  delta=1e-5,
188 ) -> np.ndarray:
189  return numericalDerivative11(lambda x: h(x1, x2, x, x4, x5, x6), x3, delta)
190 
191 
193  h: Callable[[X1, X2, X3, X4, X5, X6], Y],
194  x1: X1,
195  x2: X2,
196  x3: X3,
197  x4: X4,
198  x5: X5,
199  x6: X6,
200  delta=1e-5,
201 ) -> np.ndarray:
202  return numericalDerivative11(lambda x: h(x1, x2, x3, x, x5, x6), x4, delta)
203 
204 
206  h: Callable[[X1, X2, X3, X4, X5, X6], Y],
207  x1: X1,
208  x2: X2,
209  x3: X3,
210  x4: X4,
211  x5: X5,
212  x6: X6,
213  delta=1e-5,
214 ) -> np.ndarray:
215  return numericalDerivative11(lambda x: h(x1, x2, x3, x4, x, x6), x5, delta)
216 
217 
219  h: Callable[[X1, X2, X3, X4, X5, X6], Y],
220  x1: X1,
221  x2: X2,
222  x3: X3,
223  x4: X4,
224  x5: X5,
225  x6: X6,
226  delta=1e-5,
227 ) -> np.ndarray:
228  return numericalDerivative11(lambda x: h(x1, x2, x3, x4, x5, x), x6, delta)
gtsam::utils.numerical_derivative.numericalDerivative63
np.ndarray numericalDerivative63(Callable[[X1, X2, X3, X4, X5, X6], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, X6 x6, delta=1e-5)
Definition: numerical_derivative.py:179
gtsam::utils.numerical_derivative.numericalDerivative61
np.ndarray numericalDerivative61(Callable[[X1, X2, X3, X4, X5, X6], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, X6 x6, delta=1e-5)
Definition: numerical_derivative.py:153
gtsam::utils.numerical_derivative.numericalDerivative66
np.ndarray numericalDerivative66(Callable[[X1, X2, X3, X4, X5, X6], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, X6 x6, delta=1e-5)
Definition: numerical_derivative.py:218
gtsam::utils.numerical_derivative.numericalDerivative64
np.ndarray numericalDerivative64(Callable[[X1, X2, X3, X4, X5, X6], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, X6 x6, delta=1e-5)
Definition: numerical_derivative.py:192
gtsam::utils.numerical_derivative.numericalDerivative11
np.ndarray numericalDerivative11(Callable[[X], Y] h, X x, delta=1e-5)
Definition: numerical_derivative.py:50
gtsam::utils.numerical_derivative.numericalDerivative65
np.ndarray numericalDerivative65(Callable[[X1, X2, X3, X4, X5, X6], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, X6 x6, delta=1e-5)
Definition: numerical_derivative.py:205
type
Definition: pytypes.h:1525
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::utils.numerical_derivative.numericalDerivative41
np.ndarray numericalDerivative41(Callable[[X1, X2, X3, X4], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, delta=1e-5)
Definition: numerical_derivative.py:99
h
const double h
Definition: testSimpleHelicopter.cpp:19
gtsam::utils.numerical_derivative.numericalDerivative52
np.ndarray numericalDerivative52(Callable[[X1, X2, X3, X4, X5], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, delta=1e-5)
Definition: numerical_derivative.py:129
gtsam::utils.numerical_derivative.local
np.ndarray local(Y a, Y b)
Definition: numerical_derivative.py:33
gtsam::utils.numerical_derivative.numericalDerivative43
np.ndarray numericalDerivative43(Callable[[X1, X2, X3, X4], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, delta=1e-5)
Definition: numerical_derivative.py:111
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::utils.numerical_derivative.numericalDerivative62
np.ndarray numericalDerivative62(Callable[[X1, X2, X3, X4, X5, X6], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, X6 x6, delta=1e-5)
Definition: numerical_derivative.py:166
gtsam::utils.numerical_derivative.numericalDerivative44
np.ndarray numericalDerivative44(Callable[[X1, X2, X3, X4], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, delta=1e-5)
Definition: numerical_derivative.py:117
isinstance
bool isinstance(handle obj)
Definition: pytypes.h:842
gtsam::utils.numerical_derivative.numericalDerivative33
np.ndarray numericalDerivative33(Callable[[X1, X2, X3], Y] h, X1 x1, X2 x2, X3 x3, delta=1e-5)
Definition: numerical_derivative.py:93
gtsam::utils.numerical_derivative.numericalDerivative22
np.ndarray numericalDerivative22(Callable[[X1, X2], Y] h, X1 x1, X2 x2, delta=1e-5)
Definition: numerical_derivative.py:75
gtsam::utils.numerical_derivative.numericalDerivative32
np.ndarray numericalDerivative32(Callable[[X1, X2, X3], Y] h, X1 x1, X2 x2, X3 x3, delta=1e-5)
Definition: numerical_derivative.py:87
gtsam::utils.numerical_derivative.numericalDerivative31
np.ndarray numericalDerivative31(Callable[[X1, X2, X3], Y] h, X1 x1, X2 x2, X3 x3, delta=1e-5)
Definition: numerical_derivative.py:81
gtsam::utils.numerical_derivative.numericalDerivative21
np.ndarray numericalDerivative21(Callable[[X1, X2], Y] h, X1 x1, X2 x2, delta=1e-5)
Definition: numerical_derivative.py:69
gtsam::utils.numerical_derivative.numericalDerivative42
np.ndarray numericalDerivative42(Callable[[X1, X2, X3, X4], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, delta=1e-5)
Definition: numerical_derivative.py:105
gtsam::utils.numerical_derivative.numericalDerivative53
np.ndarray numericalDerivative53(Callable[[X1, X2, X3, X4, X5], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, delta=1e-5)
Definition: numerical_derivative.py:135
gtsam::utils.numerical_derivative.numericalDerivative51
np.ndarray numericalDerivative51(Callable[[X1, X2, X3, X4, X5], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, delta=1e-5)
Definition: numerical_derivative.py:123
gtsam::utils.numerical_derivative.numericalDerivative55
np.ndarray numericalDerivative55(Callable[[X1, X2, X3, X4, X5], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, delta=1e-5)
Definition: numerical_derivative.py:147
gtsam::utils.numerical_derivative.numericalDerivative54
np.ndarray numericalDerivative54(Callable[[X1, X2, X3, X4, X5], Y] h, X1 x1, X2 x2, X3 x3, X4 x4, X5 x5, delta=1e-5)
Definition: numerical_derivative.py:141


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:51