Add standoff transform creation tool (OpenwaterHealth#147)

Author: ebrahimebrahim

src/openlifu/geo.py

@@ -215,3 +215,32 @@ def spherical_coordinate_basis(th:float, phi:float) -> np.ndarray:
         [np.cos(th)*np.cos(phi), np.cos(th)*np.sin(phi), -np.sin(th)],
         [-np.sin(phi), np.cos(phi), 0],
     ])
+
+# === General coordinate transformation utiltiies ===
+
+def create_standoff_transform(z_offset:float, dzdy:float) -> np.ndarray:
+    """Create a standoff transform based on a z_offset and a dzdy value.
+
+    A "standoff transform" applies a displacement in transducer space that moves a transducer to where it would
+    be situated with the standoff in place. The idea is that if you start with a transform that places a transducer
+    directly against skin, then pre-composing that transform by a "standoff transform" serves to nudge the transducer
+    such that there is space for the standoff to be between it and the skin.
+
+    This function assumes that the standoff is laterally symmetric, has some thickness, and can raise the bottom of
+    the transducer a bit more than the top. The `z_offset` is the thickness in the middle of the standoff,
+    while the `dzdy` is the elevational slope.
+
+    Args:
+        z_offset: Thickness in the middle of the standoff
+        dzdy: Slope of the standoff, as axial displacement per unit elevational displacement. A positive number
+            here means that the bottom of the transducer is raised a little bit more than the top.
+
+    Returns a 4x4 matrix representing a rigid transform in whatever units z_offset was provided in.
+    """
+    angle = np.arctan(dzdy)
+    return np.array([
+        [1,0,0,0],
+        [0,np.cos(angle),-np.sin(angle),0],
+        [0,np.sin(angle),np.cos(angle),-z_offset],
+        [0,0,0,1],
+    ], dtype=float)

tests/test_geo.py

@@ -6,6 +6,7 @@
     Point,
     cartesian_to_spherical,
     cartesian_to_spherical_vectorized,
+    create_standoff_transform,
     spherical_coordinate_basis,
     spherical_to_cartesian,
     spherical_to_cartesian_vectorized,
@@ -87,3 +88,15 @@ def test_spherical_coordinate_basis():
     assert np.allclose(np.diff(r_hat / point), 0) # verify that r_hat is a scalar multiple of the cartesian coords
     assert cartesian_to_spherical_vectorized(point + 0.01*phi_hat)[2] > phi # verify phi_hat points along increasing phi
     assert cartesian_to_spherical_vectorized(point + 0.01*theta_hat)[1] > th # verify theta_hat points along increasing theta
+
+def test_create_standoff_transform():
+    z_offset = 3.2
+    dzdy = 0.15
+    t = create_standoff_transform(z_offset, dzdy)
+    assert np.allclose(t[:3,:3] @ t[:3,:3].T, np.eye(3)) # it's an orthonormal transform
+    assert np.allclose(np.linalg.det(t[:3,:3]), 1.0) # orientation preserving
+    assert np.allclose(t @ np.array([0,0,0,1]), np.array([0,0,-z_offset,1.])) # translates the origin correctly
+    new_x_axis = (t @ np.array([1,0,0,1]) - t @ np.array([0,0,0,1]))[:3]
+    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