diff --git a/python/gtsam/tests/test_numerical_derivative.py b/python/gtsam/tests/test_numerical_derivative.py index 47d2595856..3a6006ce7a 100644 --- a/python/gtsam/tests/test_numerical_derivative.py +++ b/python/gtsam/tests/test_numerical_derivative.py @@ -11,10 +11,16 @@ # 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 +import numpy as np +from gtsam import Point3, Pose3, Rot3 +from gtsam.utils.numerical_derivative import ( + local, + numericalDerivative11, + numericalDerivative21, + numericalDerivative22, + numericalDerivative33, +) class TestNumericalDerivatives(unittest.TestCase): @@ -35,6 +41,43 @@ def h(x): numerical_derivative, analytical_derivative, rtol=1e-5 ) + def test_scalar_inputs(self): + # Retraction must keep scalar inputs scalar for Python and NumPy numbers. + for scalar_type in (float, int, np.float32, np.float64, np.int64): + for value in (0, -3, 3): + with self.subTest(scalar_type=scalar_type, value=value): + x = scalar_type(value) + actual = numericalDerivative11(lambda value: value ** 2, x) + np.testing.assert_allclose(actual, [[2.0 * value]], rtol=1e-5) + + def test_local_mixed_scalar_types(self): + # Perturbations can promote integer or low-precision scalar callbacks. + scalar_types = (float, int, np.float32, np.float64, np.int64) + for first_type in scalar_types: + for second_type in scalar_types: + with self.subTest(first_type=first_type, second_type=second_type): + np.testing.assert_array_equal( + local(first_type(-1), second_type(2)), [3.0] + ) + + def test_local_rejects_scalar_array_mismatch(self): + for a, b in ((1.0, np.array([1.0])), (np.array([1.0]), 1.0)): + with self.subTest(a=type(a), b=type(b)), self.assertRaises(TypeError): + local(a, b) + + def test_scalar_input_with_scalar_only_function(self): + # A scalar callback must not receive a one-element array. + def square(value): + self.assertTrue(np.isscalar(value)) + return value ** 2 + + np.testing.assert_allclose(numericalDerivative11(square, 3.0), [[6.0]]) + + def test_scalar_input_vector_output(self): + # Scalar variables can also be used in vector-valued custom residuals. + actual = numericalDerivative11(lambda value: np.array([value, value ** 2]), 3.0) + np.testing.assert_allclose(actual, [[1.0], [6.0]]) + def test_numericalDerivative11_vector(self): # Test function of one vector variable def h(x): @@ -122,4 +165,4 @@ def h(pose:Pose3, point:np.ndarray): np.testing.assert_allclose(numerical_H2, analytic_H2, rtol=1e-5) if __name__ == "__main__": - unittest.main() \ No newline at end of file + unittest.main() diff --git a/python/gtsam/utils/numerical_derivative.py b/python/gtsam/utils/numerical_derivative.py index 68859d1ff3..4e1476f35a 100644 --- a/python/gtsam/utils/numerical_derivative.py +++ b/python/gtsam/utils/numerical_derivative.py @@ -31,19 +31,25 @@ def local(a: Y, b: Y) -> np.ndarray: + if isinstance(a, (float, int, np.floating, np.integer)) and isinstance( + b, (float, int, np.floating, np.integer) + ): + # Numeric scalars share a one-dimensional tangent space. + return np.array([float(b) - float(a)]) 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, np.floating, np.integer)): - # Represent scalar values as a 1D "tangent vector" of length 1. - return np.array([float(b) - float(a)]) # 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)): + if isinstance(a, (float, int, np.floating, np.integer)): + # Use double precision, as in local(); do not quantize steps to an + # integer or low-precision input dtype. Arbitrary precision is not retained. + return float(a) + xi[0] + if isinstance(a, np.ndarray): return a + xi return a.retract(xi)