From 4e3ebf0d17d3a3a87988bdc10b88c817cb894571 Mon Sep 17 00:00:00 2001 From: Amit T Subhash Date: Sun, 21 Jun 2026 16:10:57 +0530 Subject: [PATCH] feat: port lps2sph/sph2lps coordinate transforms from MATLAB Port fus.seg.lps2sph and fus.seg.sph2lps from the legacy MATLAB toolbox (opw_neuromod_sw) into openlifu.geo.transforms, with scalar and vectorized variants matching the existing cartesian/spherical converters. Faithfully preserves the MATLAB convention (degrees, azimuth+90 offset, elevation angle). Adds golden-value, round-trip, edge-case (r=0), and vectorized-consistency tests to test_geo.py. Refs #463 --- src/openlifu/geo/__init__.py | 8 ++++ src/openlifu/geo/transforms.py | 80 ++++++++++++++++++++++++++++++++ tests/test_geo.py | 84 ++++++++++++++++++++++++++++++++++ 3 files changed, 172 insertions(+) diff --git a/src/openlifu/geo/__init__.py b/src/openlifu/geo/__init__.py index ac5c326d..cc8539b2 100644 --- a/src/openlifu/geo/__init__.py +++ b/src/openlifu/geo/__init__.py @@ -6,9 +6,13 @@ cartesian_to_spherical, cartesian_to_spherical_vectorized, create_standoff_transform, + lps_to_spherical, + lps_to_spherical_vectorized, spherical_coordinate_basis, spherical_to_cartesian, spherical_to_cartesian_vectorized, + spherical_to_lps, + spherical_to_lps_vectorized, ) __all__ = [ @@ -18,6 +22,10 @@ "spherical_to_cartesian", "cartesian_to_spherical_vectorized", "spherical_to_cartesian_vectorized", + "lps_to_spherical", + "spherical_to_lps", + "lps_to_spherical_vectorized", + "spherical_to_lps_vectorized", "spherical_coordinate_basis", "create_standoff_transform", ] diff --git a/src/openlifu/geo/transforms.py b/src/openlifu/geo/transforms.py index 77833e94..c3f76673 100644 --- a/src/openlifu/geo/transforms.py +++ b/src/openlifu/geo/transforms.py @@ -100,6 +100,86 @@ def spherical_to_cartesian_vectorized(p: np.ndarray) -> np.ndarray: ) +def lps_to_spherical(l: float, p: float, s: float) -> Tuple[float, float, float]: + """Convert LPS (left, posterior, superior) coordinates to LPS spherical coordinates. + + This is a port of the MATLAB function ``fus.seg.lps2sph``. Note that it uses a different spherical + convention than `cartesian_to_spherical`: the angles here are in degrees, ``th`` is an azimuthal angle + offset so that the anterior (nose) direction is 0 degrees, and ``phi`` is an elevation angle rather than + the polar angle off the z-axis. + + Args: l, p, s are the left, posterior, and superior coordinates. + Returns: th, phi, r, where + th is the azimuthal angle in degrees, increasing toward patient-left from the anterior line, in the range (-90, 270]. + phi is the elevation angle in degrees, measured above the left-posterior plane, in the range [-90, 90]. + r is the radial distance, a nonnegative float in the same units as the inputs. + """ + return ( + np.rad2deg(np.arctan2(p, l)) + 90.0, + np.rad2deg(np.arctan2(s, np.hypot(l, p))), + np.sqrt(l**2 + p**2 + s**2), + ) + + +def spherical_to_lps(th: float, phi: float, r: float) -> Tuple[float, float, float]: + """Convert LPS spherical coordinates to LPS (left, posterior, superior) coordinates. + + This is a port of the MATLAB function ``fus.seg.sph2lps`` and is the inverse of `lps_to_spherical`. + + Args: + th: the azimuthal angle in degrees, increasing toward patient-left from the anterior line + phi: the elevation angle in degrees, measured above the left-posterior plane + r: the radial distance + Returns the LPS coordinates l, p, s in the same units as r. + """ + az = np.deg2rad(th - 90.0) + el = np.deg2rad(phi) + return ( + r * np.cos(el) * np.cos(az), + r * np.cos(el) * np.sin(az), + r * np.sin(el), + ) + + +def lps_to_spherical_vectorized(p: np.ndarray) -> np.ndarray: + """Convert LPS coordinates to LPS spherical coordinates. + + Args: + p: an array of shape (...,3), where the last axis describes point LPS coordinates l, p, s. + Returns: An array of shape (...,3), where the last axis describes point LPS spherical coordinates + th, phi, r. See `lps_to_spherical` for the definitions and units of these coordinates. + """ + return np.stack( + [ + np.rad2deg(np.arctan2(p[..., 1], p[..., 0])) + 90.0, + np.rad2deg(np.arctan2(p[..., 2], np.sqrt((p[..., 0:2] ** 2).sum(axis=-1)))), + np.sqrt((p**2).sum(axis=-1)), + ], + axis=-1, + ) + + +def spherical_to_lps_vectorized(p: np.ndarray) -> np.ndarray: + """Convert LPS spherical coordinates to LPS coordinates. + + Args: + p: an array of shape (...,3), where the last axis describes point LPS spherical coordinates + th, phi, r. See `lps_to_spherical` for the definitions and units of these coordinates. + Returns: An array of shape (...,3), where the last axis describes point LPS coordinates l, p, s. + """ + az = np.deg2rad(p[..., 0] - 90.0) + el = np.deg2rad(p[..., 1]) + r = p[..., 2] + return np.stack( + [ + r * np.cos(el) * np.cos(az), + r * np.cos(el) * np.sin(az), + r * np.sin(el), + ], + axis=-1, + ) + + def spherical_coordinate_basis(th: float, phi: float) -> np.ndarray: """Return normalized spherical coordinate basis at a location with spherical polar and azimuthal coordinates (th, phi). The coordinate basis is returned as an array `basis` of shape (3,3), where the rows are the basis vectors, diff --git a/tests/test_geo.py b/tests/test_geo.py index 6ef43576..19249432 100644 --- a/tests/test_geo.py +++ b/tests/test_geo.py @@ -7,9 +7,13 @@ cartesian_to_spherical, cartesian_to_spherical_vectorized, create_standoff_transform, + lps_to_spherical, + lps_to_spherical_vectorized, spherical_coordinate_basis, spherical_to_cartesian, spherical_to_cartesian_vectorized, + spherical_to_lps, + spherical_to_lps_vectorized, ) @@ -100,3 +104,83 @@ def test_create_standoff_transform(): new_y_axis = (t @ np.array([0,1,0,1]) - t @ np.array([0,0,0,1]))[:3] assert np.allclose(new_x_axis, np.array([1.,0,0])) assert new_y_axis[2] > 0 # the y axis was rotated upward, so that the top of the transducer gets closer to the skin + + +def test_lps_to_spherical_golden(): + """Anchor lps_to_spherical against values computed from the MATLAB reference (fus.seg.lps2sph).""" + np.testing.assert_almost_equal(lps_to_spherical(0.0, 0.0, 1.0), (90.0, 90.0, 1.0)) + np.testing.assert_almost_equal(lps_to_spherical(1.0, 0.0, 0.0), (90.0, 0.0, 1.0)) + np.testing.assert_almost_equal(lps_to_spherical(0.0, 1.0, 0.0), (180.0, 0.0, 1.0)) + np.testing.assert_almost_equal(lps_to_spherical(0.0, -1.0, 0.0), (0.0, 0.0, 1.0)) + np.testing.assert_almost_equal(lps_to_spherical(3.0, 4.0, 0.0), (143.13010235, 0.0, 5.0)) + np.testing.assert_almost_equal(lps_to_spherical(1.0, 1.0, 1.0), (135.0, 35.26438968, 1.73205081)) + + +def test_spherical_to_lps_golden(): + """Anchor spherical_to_lps against values computed from the MATLAB reference (fus.seg.sph2lps).""" + np.testing.assert_almost_equal(spherical_to_lps(90.0, 90.0, 1.0), (0.0, 0.0, 1.0)) + np.testing.assert_almost_equal(spherical_to_lps(90.0, 0.0, 1.0), (1.0, 0.0, 0.0)) + np.testing.assert_almost_equal(spherical_to_lps(180.0, 0.0, 1.0), (0.0, 1.0, 0.0)) + np.testing.assert_almost_equal(spherical_to_lps(0.0, 0.0, 1.0), (0.0, -1.0, 0.0)) + + +def test_lps_spherical_inverse(): + """Verify lps_to_spherical and spherical_to_lps invert one another across all eight octants.""" + rng = np.random.default_rng(1234) + for sign_l in [-1, 1]: + for sign_p in [-1, 1]: + for sign_s in [-1, 1]: + lps = np.array([sign_l, sign_p, sign_s]) * (rng.random(size=3) + 0.1) + np.testing.assert_almost_equal(spherical_to_lps(*lps_to_spherical(*lps)), lps) + + +def test_spherical_to_lps_inverse(): + """Verify the round trip the other direction for angles in the range produced by lps_to_spherical.""" + rng = np.random.default_rng(5678) + for _ in range(20): + sph = (rng.uniform(-89.0, 269.0), rng.uniform(-89.0, 89.0), rng.uniform(0.1, 10.0)) + np.testing.assert_almost_equal(lps_to_spherical(*spherical_to_lps(*sph)), sph) + + +def test_lps_spherical_r_zero(): + """Edge case r=0: the origin has zero radius, and any zero-radius spherical point maps back to the origin.""" + th, phi, r = lps_to_spherical(0.0, 0.0, 0.0) + assert r == 0.0 + assert np.isfinite([th, phi]).all() + np.testing.assert_almost_equal(spherical_to_lps(37.0, -12.0, 0.0), (0.0, 0.0, 0.0)) + np.testing.assert_almost_equal(spherical_to_lps(180.0, 90.0, 0.0), (0.0, 0.0, 0.0)) + + +def test_lps_to_spherical_vectorized(): + """The vectorized LPS-to-spherical converter must agree with the scalar one point by point.""" + rng = np.random.default_rng(913) + points_lps = rng.normal(size=(10, 3), scale=2) + points_spherical = lps_to_spherical_vectorized(points_lps) + for point_lps, point_spherical in zip(points_lps, points_spherical): + assert np.allclose(point_spherical, np.array(lps_to_spherical(*point_lps))) + + +def test_spherical_to_lps_vectorized(): + """The vectorized spherical-to-LPS converter must agree with the scalar one point by point.""" + rng = np.random.default_rng(2024) + points_spherical = np.stack( + [ + rng.uniform(-89.0, 269.0, size=10), + rng.uniform(-89.0, 89.0, size=10), + rng.uniform(0.1, 10.0, size=10), + ], + axis=-1, + ) + points_lps = spherical_to_lps_vectorized(points_spherical) + for point_spherical, point_lps in zip(points_spherical, points_lps): + assert np.allclose(point_lps, np.array(spherical_to_lps(*point_spherical))) + + +def test_lps_spherical_vectorized_inverse(): + """The vectorized converters must invert one another over arbitrary leading dimensions.""" + rng = np.random.default_rng(77) + points_lps = rng.normal(size=(8, 5, 3), scale=3) + np.testing.assert_almost_equal( + spherical_to_lps_vectorized(lps_to_spherical_vectorized(points_lps)), + points_lps, + )