fix(mobile): preserve range-bearing observation rows

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

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)