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 @@
 
 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 @@ def test_between(self):
         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 @@ def test_inverse(self):
         #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 @@ def test_slerp(self):
         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 @@ def test_transformTo(self):
         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 @@ def test_transformFrom(self):
         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()