Merged in feature/RAM-3475_multi-bb_shift_vector (pull request #368)

RAM-3475 Add bb shift vector for multi-met

Approved-by: Randy Taylor

pylinac/winston_lutz.py

@@ -2210,9 +2210,46 @@ def plot_location(
             plt.show()
         return fig, ax
 
-    @property
-    def bb_shift_vector(self) -> Vector:
-        raise NotImplementedError("Not yet implemented")
+    def bb_shift_vector(
+        self, axes_order: str = "roll,pitch,yaw"
+    ) -> (Vector, float, float, float):
+        """Calculate the ideal shift in 6 degrees of freedom to place the BB at the isocenter.
+
+        Parameters
+        ----------
+        axes_order : str
+            The order of the axes to calculate the shift in. The order is the order of the rotations.
+            The default is 'roll,pitch,yaw'. The rule of thumb is to rotate about axes
+            with the smallest expected shift first. E.g. for a 4D couch the default value works well
+            because the roll and pitch should be small.
+
+            .. warning::
+
+                This order matters more than you think it would. Results for the yaw, pitch, and roll can
+                vary by a significant or unrealistic amount depending on the order and/or could result
+                in gimbal lock
+
+        Returns
+        -------
+        Vector
+            The ideal shift vector in mm for the cartesian coordinates. X,Y, and Z follow the pylinac coordinate convention.
+        float
+            Yaw; The ideal rotation about the Z-axis in degrees.
+        float
+            Pitch; The ideal rotation about the X-axis in degrees.
+        float
+            Roll; The ideal rotation about the Y-axis in degrees.
+
+        See Also
+        --------
+        Euler Angles: https://en.wikipedia.org/wiki/Euler_angles
+        Gimbal Lock: https://en.wikipedia.org/wiki/Gimbal_lock
+        """
+        return align_points(
+            measured_points=[bb.measured_bb_position for bb in self.bbs],
+            ideal_points=[bb.measured_field_position for bb in self.bbs],
+            axes_order=axes_order,
+        )
 
     @property
     def gantry_coll_iso_size(self) -> float:
@@ -2642,3 +2679,77 @@ def solve_3d_position_from_2d_planes(
     The good news is that the position in space is the inverse of the shift vector!
     """
     return -solve_3d_shift_vector_from_2d_planes(xs, ys, thetas, phis, scale)
+
+
+def conventional_to_euler_notation(axes_resolution: str) -> str:
+    """Convert conventional understandings of 6DOF rotations into Euler notation.
+
+    Ensures we don't mix up x, y, z with the pylinac coordinate system.
+    """
+    EULER = {
+        # FROM THE COUCH PERSPECTIVE
+        "pitch": "x",  # positive pitch goes up
+        "yaw": "z",
+        "roll": "y",  # positive angle rolls to the right
+    }
+    axes = axes_resolution.split(",")
+    euler = "".join([EULER[a.strip()] for a in axes])
+    return euler
+
+
+def align_points(
+    measured_points: Sequence[Point],
+    ideal_points: Sequence[Point],
+    axes_order: str = "roll,pitch,yaw",
+) -> (Vector, float, float, float):
+    """
+    Aligns a set of measured points to a set of ideal points in 3D space, returning the
+    translation and yaw rotation needed.
+
+    Parameters
+    ----------
+    measured_points : np.ndarray
+        The measured points as an Nx3 numpy array.
+    ideal_points : np.ndarray
+        The ideal points as an Nx3 numpy array.
+    axes_order : str
+        The order in which to resolve the axes.
+        Resolution is **not** independent of the axes order. I.e. doing 'yaw,pitch,roll' may result
+        in a different outcome.
+
+    Returns
+    -------
+    Vector, float, float, float
+        The vector is the cartesian translation (dx, dy, dz) and yaw, pitch, and roll angle in degrees required to align the measured points
+        to the ideal points.
+    """
+    # convert from Point to stacked array (x, y, z) x N
+    measured_array = [[p.x, p.y, p.z] for p in measured_points]
+    ideal_array = [[p.x, p.y, p.z] for p in ideal_points]
+    # Ensure the points are centered at their centroids
+    measured_centroid = np.mean(measured_array, axis=0)
+    ideal_centroid = np.mean(ideal_array, axis=0)
+    measured_centered = measured_array - measured_centroid
+    ideal_centered = ideal_array - ideal_centroid
+
+    # Compute the covariance matrix
+    H = measured_centered.T @ ideal_centered
+
+    # Singular Value Decomposition
+    U, _, Vt = np.linalg.svd(H)
+    rotation_matrix = Vt.T @ U.T
+
+    # Ensure a right-handed coordinate system
+    if np.linalg.det(rotation_matrix) < 0:
+        Vt[2, :] *= -1
+        rotation_matrix = Vt.T @ U.T
+
+    # Compute the euler angles
+    rotation = Rotation.from_matrix(rotation_matrix)
+    euler = conventional_to_euler_notation(axes_order)
+    roll, pitch, yaw = rotation.as_euler(euler, degrees=True)
+
+    rotated_measured_centroid = rotation.apply(measured_centroid)
+    translation = ideal_centroid - rotated_measured_centroid
+
+    return Vector(*translation), yaw, pitch, roll

tests_basic/test_winstonlutz.py

@@ -6,14 +6,16 @@
 import shutil
 import tempfile
 from pathlib import Path
+from typing import Sequence
 from unittest import TestCase
 
 import matplotlib.pyplot as plt
 import numpy as np
+from scipy.spatial.transform import Rotation
 
 from pylinac import WinstonLutz
 from pylinac.core.array_utils import create_dicom_files_from_3d_array
-from pylinac.core.geometry import Vector, vector_is_close
+from pylinac.core.geometry import Point, Vector, vector_is_close
 from pylinac.core.image_generator import (
     AS1200Image,
     GaussianFilterLayer,
@@ -27,7 +29,9 @@
     Axis,
     WinstonLutz2D,
     WinstonLutzResult,
+    align_points,
     bb_projection_with_rotation,
+    conventional_to_euler_notation,
     solve_3d_position_from_2d_planes,
     solve_3d_shift_vector_from_2d_planes,
 )
@@ -43,6 +47,180 @@
 TEST_DIR = "Winston-Lutz"
 
 
+def apply_rotation_to_points(
+    points: list[Point], angle: float | Sequence[float], axis: str
+) -> list[Point]:
+    # convert from explicit axis to Euler angle; i.e. "roll" -> "z"
+    # for multiple axis rotations, separate by commas and also the
+    # angles must be in the same order as the axes and the same length
+    euler = conventional_to_euler_notation(axes_resolution=axis)
+    rotation = Rotation.from_euler(euler, angle, degrees=True)
+    return [Point(*rotation.apply([p.x, p.y, p.z])) for p in points]
+
+
+class TestAlignPoints3D(TestCase):
+    def test_perfect(self):
+        v, yaw, pitch, roll = align_points(
+            measured_points=[Point(0, 0, 0), Point(1, 1, 1)],
+            ideal_points=[Point(0, 0, 0), Point(1, 1, 1)],
+        )
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, 0)
+
+    def test_1mm_offset_left(self):
+        # 1mm offset to the left means we need to shift 1mm to the right
+        v, yaw, pitch, roll = align_points(
+            measured_points=[Point(1, 3, 0), Point(1, 0, 0)],
+            ideal_points=[Point(0, 3, 0), Point(0, 0, 0)],
+        )
+        self.assertAlmostEqual(v.x, -1)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, 0)
+
+    def test_1mm_offset_in(self):
+        # 1mm offset in means we need to shift 1mm out
+        v, yaw, pitch, roll = align_points(
+            measured_points=[Point(0, 1, 0)], ideal_points=[Point(0, 0, 0)]
+        )
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, -1)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, 0)
+
+    def test_1mm_up(self):
+        # 1mm offset up means we need to shift 1mm down
+        v, yaw, pitch, roll = align_points(
+            measured_points=[Point(0, 0, 1)], ideal_points=[Point(0, 0, 0)]
+        )
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, -1)
+        self.assertAlmostEqual(yaw, 0)
+
+    def test_1mm_offset_left_and_3_degree_yaw(self):
+        # 1mm offset to the left means we need to shift 1mm to the right
+        ideal_points = [Point(0, 0, 0), Point(-1, 1, 0), Point(1, 0, 1)]
+        measured_offset = [Point(-1, 0, 0), Point(-2, 1, 0), Point(0, 0, 1)]
+        measured_points = apply_rotation_to_points(measured_offset, 3, "yaw")
+        v, yaw, pitch, roll = align_points(
+            measured_points=measured_points, ideal_points=ideal_points
+        )
+        self.assertAlmostEqual(v.x, 1)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, -3)
+
+    def test_90_rotation(self):
+        ideal_3_long_dots = [
+            Point(0, 0, 0),
+            Point(0, 1, 0),
+            Point(0, -1, 0),
+        ]  # iso and 1mm in
+        measured_3_lateral_dots = apply_rotation_to_points(ideal_3_long_dots, 90, "yaw")
+        v, yaw, pitch, roll = align_points(
+            measured_points=measured_3_lateral_dots, ideal_points=ideal_3_long_dots
+        )
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, -90)
+
+    def test_45_rotation(self):
+        # test a 45 degree rotation; one bb is at iso perfectly, the other is 0.707/0.707 to the right and in but should be fully right
+        measured_45 = [
+            Point(0, 0, 0),
+            Point(0.707, 0.707, 0),
+            Point(-0.707, -0.707, 0),
+        ]  # iso and 1mm to the right
+        ideal_lateral = [
+            Point(0, 0, 0),
+            Point(1, 0, 0),
+            Point(-1, 0, 0),
+        ]  # iso and 1mm right
+        v, yaw, pitch, roll = align_points(
+            measured_points=measured_45, ideal_points=ideal_lateral
+        )
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, -45)
+
+    def test_10_degree_pitch(self):
+        # test a 10 degree pitch; the bb is at iso but the other 2 are 1mm up and down
+        ideal_flat = [Point(0, 0, 0), Point(0, 1, 0)]
+        measured_10_pitch = apply_rotation_to_points(ideal_flat, 10, "pitch")
+        v, yaw, pitch, roll = align_points(
+            measured_points=measured_10_pitch, ideal_points=ideal_flat
+        )
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, 0)
+        self.assertAlmostEqual(roll, 0)
+        self.assertAlmostEqual(pitch, -10)
+
+    def test_10_degree_roll(self):
+        ideal = [Point(0, 0, 0), Point(0, 0, 1), Point(0, 1, 1)]  # up 1
+        measured = apply_rotation_to_points(ideal, 10, "roll")
+        v, yaw, pitch, roll = align_points(measured_points=measured, ideal_points=ideal)
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, 0)
+        self.assertAlmostEqual(roll, -10)
+        self.assertAlmostEqual(pitch, 0)
+
+    def test_10_degree_yaw(self):
+        ideal = [Point(0, 0, 0), Point(1, 0, 0), Point(1, 1, 0)]
+        measured = apply_rotation_to_points(ideal, 10, "yaw")
+        v, yaw, pitch, roll = align_points(measured_points=measured, ideal_points=ideal)
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, -10)
+        self.assertAlmostEqual(roll, 0)
+        self.assertAlmostEqual(pitch, 0)
+
+    def test_pitch_and_yaw(self):
+        # here's where things get hairy based on resolution order
+        ideal = [Point(0, 0, 0), Point(0, 1, 0), Point(1, 0, 1)]
+        measured = apply_rotation_to_points(ideal, (10, 20), "pitch,yaw")
+        v, yaw, pitch, roll = align_points(measured_points=measured, ideal_points=ideal)
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        # these aren't correct so much as we test the function and constancy
+        self.assertAlmostEqual(yaw, -20.3, delta=0.1)
+        self.assertAlmostEqual(roll, -3.45, delta=0.1)
+        self.assertAlmostEqual(pitch, -9.4, delta=1)
+
+    def test_roll_and_yaw(self):
+        ideal = [Point(0, 0, 0), Point(0, 1, 0), Point(1, 0, 1)]
+        measured = apply_rotation_to_points(ideal, (10, 20), "roll,yaw")
+        v, yaw, pitch, roll = align_points(measured_points=measured, ideal_points=ideal)
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        # these aren't correct so much as we test the function and constancy
+        self.assertAlmostEqual(yaw, -19.7, delta=0.1)
+        self.assertAlmostEqual(roll, -9.4, delta=0.1)
+        self.assertAlmostEqual(pitch, 3.4, delta=0.1)
+
+    def test_roll_pitch_yaw(self):
+        ideal = [Point(0, 0, 0), Point(0, 1, 0), Point(1, 0, 1)]
+        measured = apply_rotation_to_points(ideal, (10, 12, 14), "roll,pitch,yaw")
+        v, yaw, pitch, roll = align_points(measured_points=measured, ideal_points=ideal)
+        self.assertAlmostEqual(v.x, 0)
+        self.assertAlmostEqual(v.y, 0)
+        self.assertAlmostEqual(v.z, 0)
+        self.assertAlmostEqual(yaw, -16.1, delta=0.1)
+        self.assertAlmostEqual(roll, -12.7, delta=0.1)
+        self.assertAlmostEqual(pitch, -9, delta=0.1)
+
+
 class Test2Dto3DPositions(TestCase):
     def test_unequal_lengths(self):
         with self.assertRaises(ValueError):