Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 47 additions & 4 deletions python/gtsam/tests/test_numerical_derivative.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -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):
Expand Down Expand Up @@ -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()
unittest.main()
14 changes: 10 additions & 4 deletions python/gtsam/utils/numerical_derivative.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down