From 9dbab04a32575d5b3468e36c9e65c85f68a358f3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 20 Sep 2024 18:12:06 -0700
Subject: [PATCH] Tests with some help from chatgpt

---
 .../gtsam/tests/test_numerical_derivative.py  | 125 ++++++++++++++++++
 1 file changed, 125 insertions(+)
 create mode 100644 python/gtsam/tests/test_numerical_derivative.py

diff --git a/python/gtsam/tests/test_numerical_derivative.py b/python/gtsam/tests/test_numerical_derivative.py
new file mode 100644
index 000000000..9bbdec277
--- /dev/null
+++ b/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 testing scenarios.
+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()
\ No newline at end of file