From 1482563c0608bd499ffc5b85dc544a3ef1fbbf67 Mon Sep 17 00:00:00 2001 From: Afloat <132809511+Afloat16@users.noreply.github.com> Date: Fri, 9 Oct 2026 19:37:27 +0800 Subject: [PATCH] fix(mobile): preserve range-bearing observation rows Preserve observation rows for all-landmark measurements and explicit pose batches, including singleton maps and singleton batches. Retain scalar explicit-landmark vectors and add analytic native sensor/reading/EKF regressions. AI assistance: OpenAI Codex (gpt-6.1-sol). --- src/roboticstoolbox/mobile/sensors.py | 20 ++++-- tests/test_sensor_observation_shape.py | 98 ++++++++++++++++++++++++++ 2 files changed, 113 insertions(+), 5 deletions(-) create mode 100644 tests/test_sensor_observation_shape.py diff --git a/src/roboticstoolbox/mobile/sensors.py b/src/roboticstoolbox/mobile/sensors.py index c92b70742..96892c765 100644 --- a/src/roboticstoolbox/mobile/sensors.py +++ b/src/roboticstoolbox/mobile/sensors.py @@ -6,7 +6,7 @@ import matplotlib.pyplot as plt from spatialmath import base import roboticstoolbox as rtb -from collections.abc import Iterable +from collections.abc import Iterable, Sequence """ Sensor Sensor superclass @@ -532,7 +532,11 @@ def isvisible(self, id): or self._theta_range[0] <= z[1] <= self._theta_range[1] ) - def h(self, x, landmark=None): + def h( + self, + x: Sequence[float] | np.ndarray, + landmark: int | Sequence[float] | np.ndarray | None = None, + ) -> np.ndarray: r""" Landmark observation function @@ -545,10 +549,15 @@ def h(self, x, landmark=None): Return the range and bearing to a landmark: - - ``.h(x)`` is range and bearing to all landmarks, one row per landmark + - ``.h(x)`` is range and bearing to all landmarks, one row per landmark, + including maps with just one landmark - ``.h(x, id)`` is range and bearing to landmark ``id`` - ``.h(x, p)`` is range and bearing to landmark with coordinates ``p`` + For an explicit landmark, a single vehicle pose returns a vector of two + elements. An ndarray of vehicle poses returns one row per pose, including + a batch containing just one pose. + .. runblock:: pycon >>> from roboticstoolbox import Bicycle, LandmarkMap, RangeBearingSensor @@ -569,7 +578,8 @@ def h(self, x, landmark=None): """ # get the landmarks, one per row - if isinstance(x, np.ndarray) and x.ndim == 2: + vectorized = isinstance(x, np.ndarray) and x.ndim == 2 + if vectorized: # x is Nx3 set of vehicle states, do vectorized form # used by particle filter x, y, t = x.T @@ -599,7 +609,7 @@ def h(self, x, landmark=None): np.sqrt(dx**2 + dy**2), base.angdiff(np.arctan2(dy, dx), t) ] # range & bearing as columns - if z.shape[0] == 1: + if z.shape[0] == 1 and landmark is not None and not vectorized: return z[0] else: return z diff --git a/tests/test_sensor_observation_shape.py b/tests/test_sensor_observation_shape.py new file mode 100644 index 000000000..237f46cab --- /dev/null +++ b/tests/test_sensor_observation_shape.py @@ -0,0 +1,98 @@ +"""Range-bearing observation rows for small maps and pose batches.""" + +import numpy as np +import numpy.testing as nt +import pytest + +from roboticstoolbox import EKF, Bicycle, LandmarkMap, RangeBearingSensor + + +@pytest.mark.parametrize("nlandmarks", [0, 1, 2]) +def test_all_landmark_observation_rows(nlandmarks: int) -> None: + robot = Bicycle(x0=[1.0, -2.0, 0.4]) + points = np.array([[3.0, 7.0], [4.0, -4.0]])[:, :nlandmarks] + sensor = RangeBearingSensor(robot, LandmarkMap(points)) + delta = points.T - robot.x[:2] + expected = np.column_stack( + (np.linalg.norm(delta, axis=1), np.arctan2(delta[:, 1], delta[:, 0]) - 0.4) + ) + + observed = sensor.h(robot.x) + nt.assert_equal(observed.shape, (nlandmarks, 2)) + nt.assert_allclose(observed, expected, atol=1e-12) + + +@pytest.mark.parametrize( + "limits, expected_visible", + [ + ({}, True), + ({"range": 10.0}, True), + ({"angle": np.pi}, True), + ({"range": 10.0, "angle": np.pi}, True), + ({"range": 1.0}, False), + ({"angle": 0.2}, False), + ], +) +def test_single_landmark_visibility_and_reading( + limits: dict[str, float], expected_visible: bool +) -> None: + robot = Bicycle(x0=[1.0, -2.0, 0.4]) + sensor = RangeBearingSensor(robot, LandmarkMap(np.array([[3.0], [4.0]])), **limits) + expected = [np.sqrt(40.0), np.arctan2(6.0, 2.0) - 0.4] + + visible = sensor.visible() + nt.assert_equal(len(visible), int(expected_visible)) + if expected_visible: + nt.assert_allclose(visible[0][0], expected, atol=1e-12) + nt.assert_equal(visible[0][1], 0) + + observation, landmark_id = sensor.reading() + if expected_visible: + nt.assert_equal(landmark_id, 0) + nt.assert_allclose(observation, expected, atol=1e-12) + else: + nt.assert_equal(observation is None, True) + nt.assert_equal(landmark_id is None, True) + + +@pytest.mark.parametrize("nposes", [0, 1, 3]) +def test_explicit_landmark_pose_batch_rows(nposes: int) -> None: + robot = Bicycle(x0=[1.0, -2.0, 0.4]) + sensor = RangeBearingSensor(robot, LandmarkMap(np.array([[3.0], [4.0]]))) + poses = np.tile(robot.x, (nposes, 1)) + expected = np.tile([np.sqrt(40.0), np.arctan2(6.0, 2.0) - 0.4], (nposes, 1)) + + observed = sensor.h(poses, 0) + nt.assert_equal(observed.shape, (nposes, 2)) + nt.assert_allclose(observed, expected, atol=1e-12) + + +def test_explicit_landmark_scalar_pose_shape() -> None: + robot = Bicycle(x0=[1.0, -2.0, 0.4]) + point = np.array([3.0, 4.0]) + sensor = RangeBearingSensor(robot, LandmarkMap(point[:, None])) + expected = [np.sqrt(40.0), np.arctan2(6.0, 2.0) - 0.4] + for landmark in [0, point]: + observed = sensor.h(robot.x, landmark) + nt.assert_equal(observed.shape, (2,)) + nt.assert_allclose(observed, expected, atol=1e-12) + + +@pytest.mark.parametrize("slam", [False, True]) +def test_single_landmark_ekf_observations(slam: bool) -> None: + start = [1.0, -2.0, 0.4] + robot = Bicycle(x0=start) + robot.control = [0.0, 0.0] + point = np.array([[3.0], [4.0]]) + sensor = RangeBearingSensor(robot, LandmarkMap(point)) + estimator = EKF( + robot=(robot, np.eye(2) * 0.01 if slam else None), + sensor=(sensor, np.eye(2) * 0.01), + P0=np.eye(3) * 0.01 if slam else None, + x0=start, + animate=False, + ) + estimator.run(T=1.0) + + nt.assert_equal(all(item.lm == 0 for item in estimator.history), True) + nt.assert_allclose(estimator.landmark_x(0), point[:, 0], atol=1e-12)