diff --git a/core/coords/transforms.py b/core/coords/transforms.py index 23ce052..7e59e18 100644 --- a/core/coords/transforms.py +++ b/core/coords/transforms.py @@ -474,6 +474,19 @@ def enu_to_body( Returns: Point expressed in the local body frame, array [x, y, z] (meters). + Notes: + - CONVENTION WARNING: ``body_origin_enu`` is the body origin expressed + in ENU -- NOT the same quantity as :func:`body_to_enu`'s + ``enu_origin_body``, which is the ENU origin expressed in the body + frame. Passing the identical array to both functions is the obvious + thing a reader tries and does NOT round-trip; it silently returns a + wrong point. The two are related by + ``enu_origin_body = -C @ body_origin_enu``, where + ``C = euler_to_rotation_matrix(roll, pitch, yaw)``. See + ``tests/core/coords/test_transforms.py::TestEnuBody`` for a pinned + failure of the naive (same-array) usage and the corrected round + trip. + Reference: Chapter 2, Eq. (2.6) - ENU to local body transformation. """ @@ -505,6 +518,19 @@ def body_to_enu( Returns: Point expressed in the ENU frame, array [east, north, up] (meters). + Notes: + - CONVENTION WARNING: ``enu_origin_body`` is the ENU origin expressed + in the body frame -- NOT the same quantity as :func:`enu_to_body`'s + ``body_origin_enu``, which is the body origin expressed in ENU. + Passing the identical array to both functions is the obvious thing + a reader tries and does NOT round-trip; it silently returns a wrong + point. The two are related by + ``enu_origin_body = -C @ body_origin_enu``, where + ``C = euler_to_rotation_matrix(roll, pitch, yaw)``. See + ``tests/core/coords/test_transforms.py::TestEnuBody`` for a pinned + failure of the naive (same-array) usage and the corrected round + trip. + Reference: Chapter 2, Eq. (2.7) - local body to ENU transformation. """ diff --git a/tests/core/coords/test_transforms.py b/tests/core/coords/test_transforms.py index c21dd1b..aecf5a0 100644 --- a/tests/core/coords/test_transforms.py +++ b/tests/core/coords/test_transforms.py @@ -17,6 +17,7 @@ import numpy as np +from core.coords.rotations import euler_to_rotation_matrix from core.coords.transforms import ( body_to_enu, body_to_map, @@ -383,6 +384,62 @@ def test_round_trip(self) -> None: recovered = body_to_enu(x_body, roll, pitch, yaw) np.testing.assert_allclose(recovered, x_enu, atol=1e-9) + def test_round_trip_with_offset_origin(self) -> None: + """enu -> body -> enu recovers the point with a non-coincident origin. + + ``enu_to_body``'s ``body_origin_enu`` and ``body_to_enu``'s + ``enu_origin_body`` are different quantities expressed in different + frames (the body origin in ENU vs. the ENU origin in body + coordinates), and the docstrings on both functions carry a + CONVENTION WARNING to that effect. This test exercises the origin + argument at all -- ``test_round_trip`` above only ever calls with + the default ``None`` -- using the correct conversion between the two, + ``enu_origin_body = -C @ body_origin_enu``. + """ + rng = np.random.default_rng(3) + for _ in range(20): + x_enu = rng.uniform(-50.0, 50.0, 3) + body_origin_enu = rng.uniform(-10.0, 10.0, 3) + roll, pitch, yaw = rng.uniform(-np.pi, np.pi, 3) + c_body_enu = euler_to_rotation_matrix(roll, pitch, yaw) + enu_origin_body = -c_body_enu @ body_origin_enu + + x_body = enu_to_body(x_enu, roll, pitch, yaw, body_origin_enu) + recovered = body_to_enu(x_body, roll, pitch, yaw, enu_origin_body) + np.testing.assert_allclose(recovered, x_enu, atol=1e-9) + + def test_same_array_as_both_origins_does_not_round_trip(self) -> None: + """Passing one origin array to both functions is a silent trap. + + ``body_origin_enu`` (body origin in ENU) and ``enu_origin_body`` (ENU + origin in the body frame) are related by a sign-flipped rotation, not + equality, so the obvious-looking call that reuses the same array for + both arguments is wrong: it silently returns the wrong point rather + than raising. This is an inequality assertion on purpose -- there is + no "correct" round-trip value to pin here, only the fact that the + naive usage must NOT recover the original point. If a future + "simplification" unifies the two arguments (e.g. treats them as + interchangeable, or auto-negates one to match the other), this test + goes green (recovered == x_enu) and should be read as a sign that the + convention -- and the CONVENTION WARNING docstrings -- changed + underneath it. + """ + x_enu = np.array([10.0, 20.0, 5.0]) + origin = np.array([1.0, 2.0, 3.0]) + roll, pitch, yaw = 0.1, 0.2, 0.3 + + x_body = enu_to_body(x_enu, roll, pitch, yaw, origin) + recovered = body_to_enu(x_body, roll, pitch, yaw, origin) + + # Measured round-trip error here is ~7.47 m (verified when this test + # was written), i.e. recovered - x_enu == -(I + C.T) @ origin, which + # is nonzero for a generic origin and rotation -- so the error is + # checkable by inspection, not just by rerunning. Assert well below + # the measured value so the test still catches the trap even if the + # specific numbers above are edited later, without pinning today's + # exact figure. + self.assertGreater(np.linalg.norm(recovered - x_enu), 1.0) + class TestENUToLLHOffset(unittest.TestCase): """A displacement in metres becomes the right offset in radians.