From d635d986c6bd154974d4ef16243a16107b4f7776 Mon Sep 17 00:00:00 2001 From: Li-Ta Hsu Date: Wed, 26 Aug 2026 07:17:17 +0800 Subject: [PATCH 1/2] Warn that enu_to_body and body_to_enu take origins in different frames body_origin_enu (enu_to_body) is the body origin expressed in ENU; enu_origin_body (body_to_enu) is the ENU origin expressed in the body frame. They are related by enu_origin_body = -C @ body_origin_enu, not equal, so passing the same array to both -- the obvious thing to try -- silently returns a wrong point (measured: full 3D round-trip error 7.47 m, with a 6.22 m error on the Up axis alone, for x=[10,20,5], origin=[1,2,3], roll/pitch/yaw=(0.1,0.2,0.3)). The signatures stay as-is per Eqs. (2.6)/(2.7); this adds a CONVENTION WARNING to both docstrings (matching the house style in core/sensors/strapdown.py's quat_to_rotmat) and two tests to TestEnuBody: a round trip through a non-None origin using the correct conversion (the existing test_round_trip only ever exercised the default None), and an inequality assertion pinning that the naive same-array usage does not round-trip, so a future change that unifies the two arguments turns it red instead of silently changing meaning. docs/ch2_equation_mapping.md and docs/CH2_QUICK_REFERENCE.md are silent on the frame of these origin arguments -- neither states it, correctly or otherwise -- so neither needed a change. --- core/coords/transforms.py | 26 ++++++++++++++ tests/core/coords/test_transforms.py | 54 ++++++++++++++++++++++++++++ 2 files changed, 80 insertions(+) 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..da02dea 100644 --- a/tests/core/coords/test_transforms.py +++ b/tests/core/coords/test_transforms.py @@ -30,6 +30,7 @@ map_to_body, ned_to_enu, ) +from core.coords.rotations import euler_to_rotation_matrix class TestLLHtoECEF(unittest.TestCase): @@ -383,6 +384,59 @@ 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); assert well below that so the test still catches the + # trap even if the specific numbers above are edited later, without + # pinning today's exact figure. + assert np.linalg.norm(recovered - x_enu) > 1.0 + class TestENUToLLHOffset(unittest.TestCase): """A displacement in metres becomes the right offset in radians. From 391d401d167e71f0d791562b61ffe2c0e1c7015e Mon Sep 17 00:00:00 2001 From: Li-Ta Hsu Date: Wed, 26 Aug 2026 07:22:36 +0800 Subject: [PATCH 2/2] Fix import order and assert style flagged in review Two follow-ups from review of the previous commit, both confirmed by rerunning the tests the review named: - from core.coords.rotations import euler_to_rotation_matrix sorted after the core.coords.transforms block, which ruff's I001 flags; moved above it (matches ruff --fix's own reordering, diffed to confirm nothing else moved). Was growing the lint ratchet from 84 to 85; test_lint_debt_only_shrinks.py now passes and a direct `ruff check --statistics` over the ratchet's SOURCE_DIRS confirms 84. - The new same-array trap test used a bare `assert`, the only one in a file that otherwise uses self.assert* 34 times; changed to self.assertGreater to match. Also folded the closed form recovered - x_enu == -(I + C.T) @ origin into the comment above it, so the ~7.47 m figure is checkable by inspection instead of only by rerunning. --- tests/core/coords/test_transforms.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/tests/core/coords/test_transforms.py b/tests/core/coords/test_transforms.py index da02dea..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, @@ -30,7 +31,6 @@ map_to_body, ned_to_enu, ) -from core.coords.rotations import euler_to_rotation_matrix class TestLLHtoECEF(unittest.TestCase): @@ -432,10 +432,13 @@ def test_same_array_as_both_origins_does_not_round_trip(self) -> None: 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); assert well below that so the test still catches the - # trap even if the specific numbers above are edited later, without - # pinning today's exact figure. - assert np.linalg.norm(recovered - x_enu) > 1.0 + # 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):