feat: port lps2sph/sph2lps coordinate transforms from MATLAB

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",
 ]

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,

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,
+    )