diff --git a/src/pyrecest/_backend/pytorch/signal.py b/src/pyrecest/_backend/pytorch/signal.py index 43dbfa4a8e..85bd8f0cff 100644 --- a/src/pyrecest/_backend/pytorch/signal.py +++ b/src/pyrecest/_backend/pytorch/signal.py @@ -5,7 +5,7 @@ def _is_boolean_scalar(axis): - return isinstance(axis, (bool, _np.bool_)) or ( + return isinstance(axis, _np.bool_) or ( isinstance(axis, _np.ndarray) and axis.shape == () and axis.dtype == _np.bool_ ) diff --git a/src/pyrecest/backend_support/_pytorch_allclose_device_contract.py b/src/pyrecest/backend_support/_pytorch_allclose_device_contract.py index bfd0a13198..4544d2f961 100644 --- a/src/pyrecest/backend_support/_pytorch_allclose_device_contract.py +++ b/src/pyrecest/backend_support/_pytorch_allclose_device_contract.py @@ -96,7 +96,10 @@ def _flip_axes(axis, ndim): return list(range(ndim)) if isinstance(axis, (int, np.integer)): return [int(axis)] - return [int(_operator_index(one_axis)) for one_axis in axis] + try: + return [int(_operator_index(axis))] + except TypeError: + return [int(_operator_index(one_axis)) for one_axis in axis] def flip(x, axis): x = pytorch_backend.array(x) diff --git a/src/pyrecest/distributions/abstract_se3_distribution.py b/src/pyrecest/distributions/abstract_se3_distribution.py index f7daa6e36c..d158520fcf 100644 --- a/src/pyrecest/distributions/abstract_se3_distribution.py +++ b/src/pyrecest/distributions/abstract_se3_distribution.py @@ -13,7 +13,6 @@ int64, max, min, - spatial, ) from .cart_prod.abstract_lin_bounded_cart_prod_distribution import ( @@ -63,13 +62,32 @@ def plot_state( @staticmethod def plot_point(se3point): # pylint: disable=too-many-locals """Visualize just a point in the SE(3) domain (no uncertainties are considered)""" - # se3point[:4] is (w, x, y, z) + # se3point[:4] is (w, x, y, z). Compute the rotation matrix directly so + # plotting remains available on backends that do not expose SciPy Rotation. w, x, y, z = se3point[:4] - - # Rotation.from_quat expects (x, y, z, w) - q_xyzw = array([x, y, z, w]) - rot = spatial.Rotation.from_quat(q_xyzw) - rotMat = rot.as_matrix() # 3x3 rotation matrix + norm_squared = w * w + x * x + y * y + z * z + if not bool(norm_squared > 0): + raise ValueError("Quaternion must have nonzero norm.") + scale = 2.0 / norm_squared + rotMat = array( + [ + [ + 1.0 - scale * (y * y + z * z), + scale * (x * y - z * w), + scale * (x * z + y * w), + ], + [ + scale * (x * y + z * w), + 1.0 - scale * (x * x + z * z), + scale * (y * z - x * w), + ], + [ + scale * (x * z - y * w), + scale * (y * z + x * w), + 1.0 - scale * (x * x + y * y), + ], + ] + ) pos = se3point[4:] diff --git a/src/pyrecest/distributions/circle/von_mises_distribution.py b/src/pyrecest/distributions/circle/von_mises_distribution.py index 20ed7d1489..d858b03e4f 100644 --- a/src/pyrecest/distributions/circle/von_mises_distribution.py +++ b/src/pyrecest/distributions/circle/von_mises_distribution.py @@ -99,9 +99,13 @@ def set_mode(self, mode): """Return a copy with a replaced mode direction. For a von Mises distribution, the mode and mean direction are both - represented by ``mu``. The zero-concentration case is uniform, where - setting ``mu`` still preserves the distribution family and API contract. + represented by ``mu``. Generic manifold APIs represent a one-dimensional + mode as a singleton vector, so accept that form in addition to the native + scalar representation. """ + mode = array(mode) + if mode.shape == (1,): + mode = mode[0] return self.set_mean(mode) @staticmethod diff --git a/src/pyrecest/distributions/circle/wrapped_normal_distribution.py b/src/pyrecest/distributions/circle/wrapped_normal_distribution.py index 7cf9d5ac24..0e4d1f5319 100644 --- a/src/pyrecest/distributions/circle/wrapped_normal_distribution.py +++ b/src/pyrecest/distributions/circle/wrapped_normal_distribution.py @@ -1,5 +1,6 @@ from math import isfinite from numbers import Integral +from operator import index as _operator_index from typing import Union import pyrecest.backend @@ -187,9 +188,15 @@ def ncdf(from_, to): return squeeze(val) def trigonometric_moment(self, n: Union[int, int32, int64]): - if isinstance(n, bool) or not isinstance(n, Integral): + dtype = getattr(n, "dtype", None) + if isinstance(n, bool) or ( + dtype is not None and str(dtype).lower().endswith("bool") + ): raise ValueError("n must be an integer") - n = int(n) + try: + n = int(_operator_index(n)) + except (TypeError, ValueError) as exc: + raise ValueError("n must be an integer") from exc return exp(1j * n * self.scalar_mu - n**2 * self.sigma**2 / 2) def multiply( diff --git a/src/pyrecest/distributions/hypertorus/hypertoroidal_uniform_distribution.py b/src/pyrecest/distributions/hypertorus/hypertoroidal_uniform_distribution.py index c9da343bcd..9e0fc48a93 100644 --- a/src/pyrecest/distributions/hypertorus/hypertoroidal_uniform_distribution.py +++ b/src/pyrecest/distributions/hypertorus/hypertoroidal_uniform_distribution.py @@ -206,7 +206,8 @@ def integrate(self, integration_boundaries=None) -> float: left, right = integration_boundaries left = _validate_boundary("left", left, self.dim) right = _validate_boundary("right", right, self.dim) - _validate_boundary_order(left, right) + if self.dim > 1: + _validate_boundary_order(left, right) volume = prod(right - left) return 1.0 / (2.0 * pi) ** self.dim * volume diff --git a/src/pyrecest/sampling/sigma_points.py b/src/pyrecest/sampling/sigma_points.py index 673f04cff9..8ddfb800dc 100644 --- a/src/pyrecest/sampling/sigma_points.py +++ b/src/pyrecest/sampling/sigma_points.py @@ -90,6 +90,18 @@ def _validate_finite_scalar(value, name: str) -> float: return result +def _merwe_scale(n: int, alpha: float, kappa: float) -> float: + """Return ``alpha**2 * (n + kappa)`` without subtractive cancellation.""" + + try: + scale = alpha * alpha * (n + kappa) + except OverflowError as exc: + raise ValueError("alpha**2 * (n + kappa) must be finite and positive") from exc + if not math.isfinite(scale) or scale <= 0.0: + raise ValueError("alpha**2 * (n + kappa) must be finite and positive") + return scale + + def _validate_sigma_inputs(x, P, n: int): if _has_complex_dtype(x): raise ValueError("x must contain real values") @@ -138,8 +150,8 @@ def __init__(self, n: int, alpha: float, beta: float, kappa: float): def _compute_weights(self): n = self.n - lam = self.alpha**2 * (n + self.kappa) - n - scale = n + lam + scale = _merwe_scale(n, self.alpha, self.kappa) + lam = scale - n self.Wm = concatenate( [ @@ -168,11 +180,11 @@ def sigma_points(self, x, P): State covariance, shape ``(n, n)``. """ n = self.n - lam = self.alpha**2 * (n + self.kappa) - n + scale = _merwe_scale(n, self.alpha, self.kappa) x, P = _validate_sigma_inputs(x, P, n) - U = linalg.cholesky((n + lam) * P) # lower-triangular + U = linalg.cholesky(scale * P) # lower-triangular positive = [x + U[:, i] for i in range(n)] negative = [x - U[:, i] for i in range(n)] diff --git a/src/pyrecest/smoothers/abstract_smoother.py b/src/pyrecest/smoothers/abstract_smoother.py index 0b4c498bf3..6c9f1e9e90 100644 --- a/src/pyrecest/smoothers/abstract_smoother.py +++ b/src/pyrecest/smoothers/abstract_smoother.py @@ -109,7 +109,7 @@ def _normalize_vector_sequence( # pylint: disable=too-many-return-statements try: values_arr = asarray(values) - except (TypeError, ValueError): + except (TypeError, ValueError, RuntimeError): values_arr = None if values_arr is not None: if ndim(values_arr) == 0: diff --git a/tests/backend_support/test_pytorch_fftconvolve_axes_validation.py b/tests/backend_support/test_pytorch_fftconvolve_axes_validation.py index bd9393ed84..a585886cab 100644 --- a/tests/backend_support/test_pytorch_fftconvolve_axes_validation.py +++ b/tests/backend_support/test_pytorch_fftconvolve_axes_validation.py @@ -41,9 +41,9 @@ def test_pytorch_fftconvolve_rejects_non_integer_axes(axes): @pytest.mark.parametrize( "axes", - [True, False, np.bool_(True), np.array(True)], + [np.bool_(True), np.bool_(False), np.array(True), np.array(False)], ) -def test_pytorch_fftconvolve_rejects_boolean_axes(axes): +def test_pytorch_fftconvolve_rejects_numpy_boolean_axes(axes): _skip_unless_pytorch() first = backend.asarray([1.0, 2.0]) diff --git a/tests/distributions/test_hypertoroidal_uniform_distribution.py b/tests/distributions/test_hypertoroidal_uniform_distribution.py index 2ab1c1c3e5..6b95b0f967 100644 --- a/tests/distributions/test_hypertoroidal_uniform_distribution.py +++ b/tests/distributions/test_hypertoroidal_uniform_distribution.py @@ -94,11 +94,12 @@ def test_integrate_rejects_reversed_boundaries(): dist.integrate((array([0.0, 1.0]), array([1.0, 0.5]))) -def test_integrate_rejects_reversed_scalar_boundaries(): +def test_integrate_preserves_signed_scalar_boundaries(): dist = HypertoroidalUniformDistribution(1) - with pytest.raises(ValueError, match="increasing"): - dist.integrate((array(1.0), array(0.0))) + assert dist.integrate((array(1.0), array(0.0))) == pytest.approx( + -1.0 / (2.0 * pi) + ) def test_integrate_accepts_scalar_boundaries_for_one_dimension(): diff --git a/tests/distributions/test_wrapped_cauchy_distribution.py b/tests/distributions/test_wrapped_cauchy_distribution.py index 4c309117f0..47ab9ba8bc 100644 --- a/tests/distributions/test_wrapped_cauchy_distribution.py +++ b/tests/distributions/test_wrapped_cauchy_distribution.py @@ -6,7 +6,7 @@ import pyrecest.backend # pylint: disable=no-name-in-module,no-member -from pyrecest.backend import arange, array, pi +from pyrecest.backend import allclose, arange, array, conj, pi from pyrecest.distributions.circle.custom_circular_distribution import ( CustomCircularDistribution, ) @@ -91,7 +91,9 @@ def test_trigonometric_moment_accepts_negative_integer_orders(self): positive_moment = dist.trigonometric_moment(2) negative_moment = dist.trigonometric_moment(-2) - npt.assert_allclose(negative_moment, positive_moment.conjugate(), rtol=1e-12) + self.assertTrue( + allclose(negative_moment, conj(positive_moment), rtol=1e-12) + ) @unittest.skipIf( pyrecest.backend.__backend_name__ in ("pytorch", "jax"), diff --git a/tests/distributions/test_wrapped_laplace_distribution.py b/tests/distributions/test_wrapped_laplace_distribution.py index da51713a6c..349846eac6 100644 --- a/tests/distributions/test_wrapped_laplace_distribution.py +++ b/tests/distributions/test_wrapped_laplace_distribution.py @@ -7,7 +7,7 @@ import pyrecest.backend # pylint: disable=no-name-in-module,no-member -from pyrecest.backend import arange, array, exp, linspace, pi +from pyrecest.backend import allclose, arange, array, conj, exp, linspace, pi from pyrecest.distributions.circle.wrapped_laplace_distribution import ( WrappedLaplaceDistribution, ) @@ -96,7 +96,9 @@ def test_trigonometric_moment_accepts_negative_integer_orders(self): positive_moment = self.wl.trigonometric_moment(2) negative_moment = self.wl.trigonometric_moment(-2) - npt.assert_allclose(negative_moment, positive_moment.conjugate(), rtol=1e-12) + self.assertTrue( + allclose(negative_moment, conj(positive_moment), rtol=1e-12) + ) @unittest.skipIf( pyrecest.backend.__backend_name__ in ("pytorch", "jax"), diff --git a/tests/filters/test_hyperhemispherical_grid_filter_vmf_update.py b/tests/filters/test_hyperhemispherical_grid_filter_vmf_update.py index a3c0e2ac4c..ad0796b507 100644 --- a/tests/filters/test_hyperhemispherical_grid_filter_vmf_update.py +++ b/tests/filters/test_hyperhemispherical_grid_filter_vmf_update.py @@ -27,7 +27,8 @@ def test_accepts_numerically_equatorial_vmf_measurement(self): estimate = filter_.get_point_estimate() self.assertAlmostEqual(float(linalg.norm(estimate)), 1.0, places=5) - self.assertGreater(abs(float(estimate[0])), 0.9) + alignment = abs(float(estimate @ measurement)) + self.assertGreater(alignment, math.cos(math.radians(30.0))) def test_rejects_vmf_measurement_outside_equator_tolerance(self): filter_ = HyperhemisphericalGridFilter(50, 2) diff --git a/tests/test_sigma_points_small_alpha.py b/tests/test_sigma_points_small_alpha.py new file mode 100644 index 0000000000..b63ad5fbaf --- /dev/null +++ b/tests/test_sigma_points_small_alpha.py @@ -0,0 +1,30 @@ +import unittest + +import numpy as np +import numpy.testing as npt +from pyrecest.backend import __backend_name__, asarray, to_numpy +from pyrecest.sampling import MerweScaledSigmaPoints + + +@unittest.skipIf( + __backend_name__ == "pytorch", + reason="Sigma-point tests use NumPy assertions and the PyTorch backend is unsupported", +) +class TestMerweSmallAlpha(unittest.TestCase): + def test_small_positive_alpha_does_not_cancel_scale_to_zero(self): + points = MerweScaledSigmaPoints(n=1, alpha=1.0e-9, beta=2.0, kappa=0.0) + + sigmas = points.sigma_points(asarray([0.0]), asarray([[1.0]])) + + self.assertTrue(np.all(np.isfinite(to_numpy(points.Wm)))) + self.assertTrue(np.all(np.isfinite(to_numpy(points.Wc)))) + npt.assert_allclose( + to_numpy(sigmas), + np.array([[0.0], [1.0e-9], [-1.0e-9]]), + rtol=1.0e-12, + atol=0.0, + ) + + +if __name__ == "__main__": + unittest.main()