12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1838 from borglab/feature/numdiff 

numdiff in python
release/4.3a0
Frank Dellaert 2024-09-23 09:56:21 -07:00 committed by GitHub
parent e52973b72d 5c80174c0b
commit 788b074ac0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 366 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
python/CustomFactors.md
View File

@ -18,12 +18,14 @@ def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -
`this` is a reference to the `CustomFactor` object. This is required because one can reuse the same
`error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of
**references** to the list of required Jacobians (see the corresponding C++ documentation). Note that
the error returned must be a 1D numpy array.
the error returned must be a 1D `numpy` array.
If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error`
method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`,
each entry of `H` can be assigned a (2D) `numpy` array, as the Jacobian for the corresponding variable.
All `numpy` matrices inside should be using `order="F"` to maintain interoperability with C++.
After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM:
```python

72
python/gtsam/tests/test_Pose3.py
View File

@ -17,63 +17,7 @@ from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import Point3, Pose3, Rot3
def numerical_derivative_pose(pose, method, delta=1e-5):
jacobian = np.zeros((6, 6))
for idx in range(6):
xplus = np.zeros(6)
xplus[idx] = delta
xminus = np.zeros(6)
xminus[idx] = -delta
pose_plus = pose.retract(xplus).__getattribute__(method)()
pose_minus = pose.retract(xminus).__getattribute__(method)()
jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
return jacobian
def numerical_derivative_2_poses(pose, other_pose, method, delta=1e-5, inputs=()):
jacobian = np.zeros((6, 6))
other_jacobian = np.zeros((6, 6))
for idx in range(6):
xplus = np.zeros(6)
xplus[idx] = delta
xminus = np.zeros(6)
xminus[idx] = -delta
pose_plus = pose.retract(xplus).__getattribute__(method)(*inputs, other_pose)
pose_minus = pose.retract(xminus).__getattribute__(method)(*inputs, other_pose)
jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
other_pose_plus = pose.__getattribute__(method)(*inputs, other_pose.retract(xplus))
other_pose_minus = pose.__getattribute__(method)(*inputs, other_pose.retract(xminus))
other_jacobian[:, idx] = other_pose_minus.localCoordinates(other_pose_plus) / (2 * delta)
return jacobian, other_jacobian
def numerical_derivative_pose_point(pose, point, method, delta=1e-5):
jacobian = np.zeros((3, 6))
point_jacobian = np.zeros((3, 3))
for idx in range(6):
xplus = np.zeros(6)
xplus[idx] = delta
xminus = np.zeros(6)
xminus[idx] = -delta
point_plus = pose.retract(xplus).__getattribute__(method)(point)
point_minus = pose.retract(xminus).__getattribute__(method)(point)
jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
if idx < 3:
xplus = np.zeros(3)
xplus[idx] = delta
xminus = np.zeros(3)
xminus[idx] = -delta
point_plus = pose.__getattribute__(method)(point + xplus)
point_minus = pose.__getattribute__(method)(point + xminus)
point_jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
return jacobian, point_jacobian
from gtsam.utils.numerical_derivative import numericalDerivative11, numericalDerivative21, numericalDerivative22
class TestPose3(GtsamTestCase):
"""Test selected Pose3 methods."""
@ -90,7 +34,8 @@ class TestPose3(GtsamTestCase):
jacobian = np.zeros((6, 6), order='F')
jacobian_other = np.zeros((6, 6), order='F')
T2.between(T3, jacobian, jacobian_other)
jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(T2, T3, 'between')
jacobian_numerical = numericalDerivative21(Pose3.between, T2, T3)
jacobian_numerical_other = numericalDerivative22(Pose3.between, T2, T3)
self.gtsamAssertEquals(jacobian, jacobian_numerical)
self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
@ -104,7 +49,7 @@ class TestPose3(GtsamTestCase):
#test jacobians
jacobian = np.zeros((6, 6), order='F')
pose.inverse(jacobian)
jacobian_numerical = numerical_derivative_pose(pose, 'inverse')
jacobian_numerical = numericalDerivative11(Pose3.inverse, pose)
self.gtsamAssertEquals(jacobian, jacobian_numerical)
def test_slerp(self):
@ -123,7 +68,8 @@ class TestPose3(GtsamTestCase):
jacobian = np.zeros((6, 6), order='F')
jacobian_other = np.zeros((6, 6), order='F')
pose0.slerp(0.5, pose1, jacobian, jacobian_other)
jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(pose0, pose1, 'slerp', inputs=[0.5])
jacobian_numerical = numericalDerivative11(lambda x: x.slerp(0.5, pose1), pose0)
jacobian_numerical_other = numericalDerivative11(lambda x: pose0.slerp(0.5, x), pose1)
self.gtsamAssertEquals(jacobian, jacobian_numerical)
self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
@ -139,7 +85,8 @@ class TestPose3(GtsamTestCase):
jacobian_pose = np.zeros((3, 6), order='F')
jacobian_point = np.zeros((3, 3), order='F')
pose.transformTo(point, jacobian_pose, jacobian_point)
jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformTo')
jacobian_numerical_pose = numericalDerivative21(Pose3.transformTo, pose, point)
jacobian_numerical_point = numericalDerivative22(Pose3.transformTo, pose, point)
self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)
@ -162,7 +109,8 @@ class TestPose3(GtsamTestCase):
jacobian_pose = np.zeros((3, 6), order='F')
jacobian_point = np.zeros((3, 3), order='F')
pose.transformFrom(point, jacobian_pose, jacobian_point)
jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformFrom')
jacobian_numerical_pose = numericalDerivative21(Pose3.transformFrom, pose, point)
jacobian_numerical_point = numericalDerivative22(Pose3.transformFrom, pose, point)
self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)

125
python/gtsam/tests/test_numerical_derivative.py Normal file
View File

@ -0,0 +1,125 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for IMU numerical_derivative module.
Author: Frank Dellaert & Joel Truher
"""
# pylint: disable=invalid-name, no-name-in-module
import unittest
import numpy as np
from gtsam import Pose3, Rot3, Point3
from gtsam.utils.numerical_derivative import numericalDerivative11, numericalDerivative21, numericalDerivative22, numericalDerivative33
class TestNumericalDerivatives(unittest.TestCase):
def test_numericalDerivative11_scalar(self):
# Test function of one variable
def h(x):
return x ** 2
x = np.array([3.0])
# Analytical derivative: dh/dx = 2x
analytical_derivative = np.array([[2.0 * x[0]]])
# Compute numerical derivative
numerical_derivative = numericalDerivative11(h, x)
# Check if numerical derivative is close to analytical derivative
np.testing.assert_allclose(
numerical_derivative, analytical_derivative, rtol=1e-5
)
def test_numericalDerivative11_vector(self):
# Test function of one vector variable
def h(x):
return x ** 2
x = np.array([1.0, 2.0, 3.0])
# Analytical derivative: dh/dx = 2x
analytical_derivative = np.diag(2.0 * x)
numerical_derivative = numericalDerivative11(h, x)
np.testing.assert_allclose(
numerical_derivative, analytical_derivative, rtol=1e-5
)
def test_numericalDerivative21(self):
# Test function of two variables, derivative with respect to first variable
def h(x1, x2):
return x1 * np.sin(x2)
x1 = np.array([2.0])
x2 = np.array([np.pi / 4])
# Analytical derivative: dh/dx1 = sin(x2)
analytical_derivative = np.array([[np.sin(x2[0])]])
numerical_derivative = numericalDerivative21(h, x1, x2)
np.testing.assert_allclose(
numerical_derivative, analytical_derivative, rtol=1e-5
)
def test_numericalDerivative22(self):
# Test function of two variables, derivative with respect to second variable
def h(x1, x2):
return x1 * np.sin(x2)
x1 = np.array([2.0])
x2 = np.array([np.pi / 4])
# Analytical derivative: dh/dx2 = x1 * cos(x2)
analytical_derivative = np.array([[x1[0] * np.cos(x2[0])]])
numerical_derivative = numericalDerivative22(h, x1, x2)
np.testing.assert_allclose(
numerical_derivative, analytical_derivative, rtol=1e-5
)
def test_numericalDerivative33(self):
# Test function of three variables, derivative with respect to third variable
def h(x1, x2, x3):
return x1 * x2 + np.exp(x3)
x1 = np.array([1.0])
x2 = np.array([2.0])
x3 = np.array([0.5])
# Analytical derivative: dh/dx3 = exp(x3)
analytical_derivative = np.array([[np.exp(x3[0])]])
numerical_derivative = numericalDerivative33(h, x1, x2, x3)
np.testing.assert_allclose(
numerical_derivative, analytical_derivative, rtol=1e-5
)
def test_numericalDerivative_with_pose(self):
# Test function with manifold and vector inputs
def h(pose:Pose3, point:np.ndarray):
return pose.transformFrom(point)
# Values from testPose3.cpp
P = Point3(0.2,0.7,-2)
R = Rot3.Rodrigues(0.3,0,0)
P2 = Point3(3.5,-8.2,4.2)
T = Pose3(R,P2)
analytic_H1 = np.zeros((3,6), order='F', dtype=float)
analytic_H2 = np.zeros((3,3), order='F', dtype=float)
y = T.transformFrom(P, analytic_H1, analytic_H2)
numerical_H1 = numericalDerivative21(h, T, P)
numerical_H2 = numericalDerivative22(h, T, P)
np.testing.assert_allclose(numerical_H1, analytic_H1, rtol=1e-5)
np.testing.assert_allclose(numerical_H2, analytic_H2, rtol=1e-5)
if __name__ == "__main__":
unittest.main()

228
python/gtsam/utils/numerical_derivative.py Normal file
View File

@ -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
Numerical derivative functions.
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.ndarray([[b - a]]) # type:ignore
# there is no common superclass for Y
return a.localCoordinates(b) # type:ignore
def retract(a, xi: np.ndarray):
if isinstance(a, (np.ndarray, float, int)):
return a + xi
return a.retract(xi)
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.ndarray:
return numericalDerivative11(lambda x: h(x, x2), x1, delta)
def numericalDerivative22(
h: Callable[[X1, X2], Y], x1: X1, x2: X2, delta=1e-5
) -> np.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
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.ndarray:
return numericalDerivative11(lambda x: h(x1, x2, x3, x4, x5, x), x6, delta)