Neuronavigation: Track multiple coils simultaneously and show stylus/probe

environment.yml

@@ -29,7 +29,7 @@ dependencies:
     - pyclaron
     - polhemusFT
     - polhemus
-    - pypolaris==0.0.7
+    - pypolaris==0.0.8
     - scikit-image==0.24.0
     - scikit-learn==1.5.0
     - torch==2.3.1

invesalius/data/bases.py

@@ -137,9 +137,7 @@ def calculate_fre(
 
     dist = np.zeros([3, 1])
     for i in range(0, 6, 2):
-        p_m, _ = dcr.corregistrate_dynamic(
-            (m_change, 0), fiducials_raw[i : i + 2], ref_mode_id, icp
-        )
+        p_m, _ = dcr.corregistrate_probe(m_change, None, fiducials_raw[i : i + 2], ref_mode_id, icp)
         dist[int(i / 2)] = np.sqrt(np.sum(np.power((p_m[:3] - fiducials[int(i / 2), :]), 2)))
 
     return float(np.sqrt(np.sum(dist**2) / 3))
@@ -176,15 +174,15 @@ def calculate_fre(
 def transform_icp(m_img: np.ndarray, m_icp: np.ndarray) -> np.ndarray:
     coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
     m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = m_icp @ coord_img
-    m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
+    m_img[1, -1] = -m_img[1, -1]
 
     return m_img
 
 
 def inverse_transform_icp(m_img: np.ndarray, m_icp: np.ndarray) -> np.ndarray:
     coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
     m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img
-    m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
+    m_img[1, -1] = -m_img[1, -1]
 
     return m_img
 
@@ -212,19 +210,16 @@ def object_registration(
         fids_raw[ic, :] = dco.dynamic_reference_m2(coords[ic, :], coords[3, :])[:3]
 
     # compute initial alignment of probe fixed in the object in source frame
-
-    # XXX: Some duplicate processing is done here: the Euler angles are calculated once by
-    #      the lines below, and then again in dco.coordinates_to_transformation_matrix.
-    #
-    a, b, g = np.radians(coords[3, 3:])
-    r_s0_raw = tr.euler_matrix(a, b, g, axes="rzyx")
-
     s0_raw = dco.coordinates_to_transformation_matrix(
         position=coords[3, :3],
         orientation=coords[3, 3:],
         axes="rzyx",
     )
 
+    # copy rotation submatrix from s0_raw
+    r_s0_raw = np.eye(4)
+    r_s0_raw[:3, :3] = s0_raw[:3, :3]
+
     # compute change of basis for object fiducials in source frame
     base_obj_raw, q_obj_raw = base_creation(fids_raw[:3, :3])
     r_obj_raw = np.identity(4)

invesalius/data/coordinates.py

@@ -254,14 +254,17 @@ def PolarisCoord(tracker_connection: "TrackerConnection", tracker_id: int, ref_m
     trans_ref = np.array(ref[6:9]).astype(float)
     coord2 = np.hstack((trans_ref, angles_ref))
 
-    obj = trck.obj.decode(const.FS_ENCODE).split(",")
-    angles_obj = np.degrees(tr.euler_from_quaternion(obj[2:6], axes="rzyx"))
-    trans_obj = np.array(obj[6:9]).astype(float)
-    coord3 = np.hstack((trans_obj, angles_obj))
+    obj_coords = []
+    for i in range(trck.objs.size()):
+        obj = trck.objs[i].decode(const.FS_ENCODE).split(",")
+        angles_obj = np.degrees(tr.euler_from_quaternion(obj[2:6], axes="rzyx"))
+        trans_obj = np.array(obj[6:9]).astype(float)
+        obj_coords.append(np.hstack((trans_obj, angles_obj)))
 
-    coord = np.vstack([coord1, coord2, coord3])
+    coord = np.vstack([coord1, coord2, *obj_coords])
+    marker_visibilities = [trck.probeID, trck.refID] + list(trck.objIDs)
 
-    return coord, [trck.probeID, trck.refID, trck.objID]
+    return coord, marker_visibilities
 
 
 def CameraCoord(tracker_connection: "TrackerConnection", tracker_id: int, ref_mode):
@@ -538,6 +541,9 @@ def DebugCoordRandom(tracker_connection: "TrackerConnection", tracker_id: int, r
     coord4 = np.array(
         [uniform(*dx), uniform(*dx), uniform(*dx), uniform(*dt), uniform(*dt), uniform(*dt)]
     )
+    coord5 = np.array(
+        [uniform(*dx), uniform(*dx), uniform(*dx), uniform(*dt), uniform(*dt), uniform(*dt)]
+    )
 
     sleep(0.15)
 
@@ -555,9 +561,9 @@ def DebugCoordRandom(tracker_connection: "TrackerConnection", tracker_id: int, r
 
     # Always make the markers visible when using debug tracker; this enables registration, as it
     # is not possible to registering without markers.
-    marker_visibilities = [True, True, True]
+    marker_visibilities = [True, True, True, True, True]
 
-    return np.vstack([coord1, coord2, coord3, coord4]), marker_visibilities
+    return np.vstack([coord1, coord2, coord3, coord4, coord5]), marker_visibilities
 
 
 def coordinates_to_transformation_matrix(