Code from Joel
Frank Dellaert
parent
9567006b6b
commit
4b04ae0944
|
|
@ -0,0 +1,228 @@
|
||||||
|
"""
|
||||||
|
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||||
|
Atlanta, Georgia 30332-0415
|
||||||
|
All Rights Reserved
|
||||||
|
|
||||||
|
See LICENSE for the license information
|
||||||
|
|
||||||
|
TestCase class with GTSAM assert utils.
|
||||||
|
Author: Joel Truher & Frank Dellaert
|
||||||
|
"""
|
||||||
|
|
||||||
|
# pylint: disable=C0103,C0114,C0116,E0611,R0913
|
||||||
|
# mypy: disable-error-code="import-untyped"
|
||||||
|
# see numericalDerivative.h
|
||||||
|
|
||||||
|
# pybind wants to wrap concrete types, which would have been
|
||||||
|
# a whole lot of them, so i reimplemented the part of this that
|
||||||
|
# I needed, using the python approach to "generic" typing.
|
||||||
|
|
||||||
|
from typing import Callable, TypeVar
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
Y = TypeVar("Y")
|
||||||
|
X = TypeVar("X")
|
||||||
|
X1 = TypeVar("X1")
|
||||||
|
X2 = TypeVar("X2")
|
||||||
|
X3 = TypeVar("X3")
|
||||||
|
X4 = TypeVar("X4")
|
||||||
|
X5 = TypeVar("X5")
|
||||||
|
X6 = TypeVar("X6")
|
||||||
|
|
||||||
|
|
||||||
|
def local(a: Y, b: Y) -> np.ndarray:
|
||||||
|
if type(a) is not type(b):
|
||||||
|
raise TypeError(f"a {type(a)} b {type(b)}")
|
||||||
|
if isinstance(a, np.ndarray):
|
||||||
|
return b - a
|
||||||
|
if isinstance(a, (float, int)):
|
||||||
|
return np.array([[b - a]]) # type:ignore
|
||||||
|
# there is no common superclass for Y
|
||||||
|
return a.localCoordinates(b) # type:ignore
|
||||||
|
|
||||||
|
|
||||||
|
def retract(a, b):
|
||||||
|
if isinstance(a, (np.ndarray, float, int)):
|
||||||
|
return a + b
|
||||||
|
return a.retract(b)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative11(h: Callable[[X], Y], x: X, delta=1e-5) -> np.ndarray:
|
||||||
|
hx: Y = h(x)
|
||||||
|
zeroY = local(hx, hx)
|
||||||
|
m = zeroY.shape[0]
|
||||||
|
zeroX = local(x, x)
|
||||||
|
N = zeroX.shape[0]
|
||||||
|
dx = np.zeros(N)
|
||||||
|
H = np.zeros((m, N))
|
||||||
|
factor: float = 1.0 / (2.0 * delta)
|
||||||
|
for j in range(N):
|
||||||
|
dx[j] = delta
|
||||||
|
dy1 = local(hx, h(retract(x, dx)))
|
||||||
|
dx[j] = -delta
|
||||||
|
dy2 = local(hx, h(retract(x, dx)))
|
||||||
|
dx[j] = 0
|
||||||
|
H[:, j] = (dy1 - dy2) * factor
|
||||||
|
return H
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative21(
|
||||||
|
h: Callable[[X1, X2], Y], x1: X1, x2: X2, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x, x2), x1, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative22(
|
||||||
|
h: Callable[[X1, X2], Y], x1: X1, x2: X2, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x), x2, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative31(
|
||||||
|
h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x, x2, x3), x1, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative32(
|
||||||
|
h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x, x3), x2, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative33(
|
||||||
|
h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x), x3, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative41(
|
||||||
|
h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x, x2, x3, x4), x1, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative42(
|
||||||
|
h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x, x3, x4), x2, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative43(
|
||||||
|
h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x, x4), x3, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative44(
|
||||||
|
h: Callable[[X1, X2, X3, X4], Y], x1: X1, x2: X2, x3: X3, x4: X4, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x3, x), x4, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative51(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x, x2, x3, x4, x5), x1, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative52(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x, x3, x4, x5), x2, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative53(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x, x4, x5), x3, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative54(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x3, x, x5), x4, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative55(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5], Y], x1: X1, x2: X2, x3: X3, x4: X4, x5: X5, delta=1e-5
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x3, x4, x), x5, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative61(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5, X6], Y],
|
||||||
|
x1: X1,
|
||||||
|
x2: X2,
|
||||||
|
x3: X3,
|
||||||
|
x4: X4,
|
||||||
|
x5: X5,
|
||||||
|
x6: X6,
|
||||||
|
delta=1e-5,
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x, x2, x3, x4, x5, x6), x1, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative62(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5, X6], Y],
|
||||||
|
x1: X1,
|
||||||
|
x2: X2,
|
||||||
|
x3: X3,
|
||||||
|
x4: X4,
|
||||||
|
x5: X5,
|
||||||
|
x6: X6,
|
||||||
|
delta=1e-5,
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x, x3, x4, x5, x6), x2, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative63(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5, X6], Y],
|
||||||
|
x1: X1,
|
||||||
|
x2: X2,
|
||||||
|
x3: X3,
|
||||||
|
x4: X4,
|
||||||
|
x5: X5,
|
||||||
|
x6: X6,
|
||||||
|
delta=1e-5,
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x, x4, x5, x6), x3, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative64(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5, X6], Y],
|
||||||
|
x1: X1,
|
||||||
|
x2: X2,
|
||||||
|
x3: X3,
|
||||||
|
x4: X4,
|
||||||
|
x5: X5,
|
||||||
|
x6: X6,
|
||||||
|
delta=1e-5,
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x3, x, x5, x6), x4, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative65(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5, X6], Y],
|
||||||
|
x1: X1,
|
||||||
|
x2: X2,
|
||||||
|
x3: X3,
|
||||||
|
x4: X4,
|
||||||
|
x5: X5,
|
||||||
|
x6: X6,
|
||||||
|
delta=1e-5,
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x3, x4, x, x6), x5, delta)
|
||||||
|
|
||||||
|
|
||||||
|
def numericalDerivative66(
|
||||||
|
h: Callable[[X1, X2, X3, X4, X5, X6], Y],
|
||||||
|
x1: X1,
|
||||||
|
x2: X2,
|
||||||
|
x3: X3,
|
||||||
|
x4: X4,
|
||||||
|
x5: X5,
|
||||||
|
x6: X6,
|
||||||
|
delta=1e-5,
|
||||||
|
) -> np.array:
|
||||||
|
return numericalDerivative11(lambda x: h(x1, x2, x3, x4, x5, x), x6, delta)
|
||||||
Loading…
Reference in New Issue