Merge pull request #1838 from borglab/feature/numdiff

numdiff in python

python/CustomFactors.md

@@ -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

python/gtsam/tests/test_Pose3.py

@@ -17,63 +17,7 @@ from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
 from gtsam import Point3, Pose3, Rot3
-
+from gtsam.utils.numerical_derivative import numericalDerivative11, numericalDerivative21, numericalDerivative22
-
-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
-
 
 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)
 

python/gtsam/tests/test_numerical_derivative.py

@@ -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()

python/gtsam/utils/numerical_derivative.py

@@ -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)