From c1b6182315337f41b038d077bafab3b7b0c7d211 Mon Sep 17 00:00:00 2001
From: Robert Ennis <rob@pupil-labs.com>
Date: Thu, 22 Aug 2024 16:56:38 +0200
Subject: [PATCH] further generalize + compress + simplify; re-tested new
 functions and they needed a tweak to work correctly again; they are back in a
 ready state

---
 alpha-lab/imu-transformations/index.md | 43 ++++++++++++++------------
 1 file changed, 23 insertions(+), 20 deletions(-)

diff --git a/alpha-lab/imu-transformations/index.md b/alpha-lab/imu-transformations/index.md
index 685e753ba..a1301ade6 100644
--- a/alpha-lab/imu-transformations/index.md
+++ b/alpha-lab/imu-transformations/index.md
@@ -94,7 +94,7 @@ To facilitate the comparison, it can be useful to represent these data streams i
 We can use data from the IMU to transform gaze from scene camera coordinates to world coordinates. This is facilitated by the `transform_scene_to_imu` and `spherical_to_cartesian_scene` functions:
 
 ```python
-def transform_scene_to_imu(coords_in_scene, translation = None):
+def transform_scene_to_imu(coords_in_scene, translation=None):
     imu_scene_rotation_diff = np.deg2rad(-90 - 12)
     scene_to_imu = np.array(
         [
@@ -112,17 +112,18 @@ def transform_scene_to_imu(coords_in_scene, translation = None):
         ]
     )
 
-    if translation:
+    if translation is not None:
         scene_to_imu_homogeneous = np.zeros((4, 4))
         scene_to_imu_homogeneous[:3, :3] = scene_to_imu
         scene_to_imu_homogeneous[:3, 3] = translation
 
-        coords_in_scene_homogeneous = cv2.convertPointsToHomogeneous(coords_in_scene.T)
+        coords_in_scene_homogeneous = cv2.convertPointsToHomogeneous(coords_in_scene)
         coords_in_imu_homogeneous = scene_to_imu_homogeneous @ coords_in_scene_homogeneous.reshape(-1, 4).T
-        coords_in_imu = cv2.convertPointsFromHomogeneous(coords_in_imu_homogeneous.T)
-        return coords_in_imu.reshape(-1, 3).T
+        coords_in_imu = cv2.convertPointsFromHomogeneous(coords_in_imu_homogeneous.T).reshape(-1, 3)
     else:
-        return scene_to_imu @ coords_in_scene
+        coords_in_imu = (scene_to_imu @ coords_in_scene.T).T
+
+    return coords_in_imu
 
 
 def spherical_to_cartesian_scene(elevations, azimuths):
@@ -157,7 +158,15 @@ def spherical_to_cartesian_scene(elevations, azimuths):
     ).T
 ```
 
-Now we have the tools to convert gaze data to world coordinates:
+Putting these together, we can build a general `transform_scene_to_world` function:
+
+```python
+def transform_scene_to_world(coords_in_scene, imu_quaternions, translation=None):
+    coords_in_imu = transform_scene_to_imu(coords_in_scene, translation)
+    return transform_imu_to_world(coords_in_imu, imu_quaternions)
+```
+
+Now we have the tools to convert 3D gaze data to world coordinates:
 
 ```python
 # The gaze data and the IMU quaternions should be sampled at the
@@ -165,8 +174,7 @@ Now we have the tools to convert gaze data to world coordinates:
 # ensure this.
 
 cart_gazes_in_scene = spherical_to_cartesian_scene(gaze_elevations, gaze_azimuths)
-gazes_in_imu = transform_scene_to_imu(cart_gazes_in_scene.T)
-gazes_in_world = transform_imu_to_world(gazes_in_imu.T, imu_quaternions)
+cart_gazes_in_world = transform_scene_to_world(cart_gazes_in_scene, imu_quaternions)
 ```
 
 ## Eyestate to World Coordinates
@@ -184,19 +192,14 @@ The [3D eyestate estimates](https://docs.pupil-labs.com/neon/data-collection/dat
 # scene camera in the IMU coordinate system. Here, we express that
 # position in millimeters.
 scene_camera_position_in_imu = np.array([0.0, -1.3, -6.62])
-
-eyeball_centers_in_imu = transform_scene_to_imu(
-    eyeball_centers.T, translation=scene_camera_position_in_imu,
-)
-eyeball_centers_in_world = transform_imu_to_world(
-    eyeball_centers_in_imu.T, imu_quaternions,
+eyeball_centers_in_world = transform_scene_to_world(
+    eyeball_centers, imu_quaternions, translation=scene_camera_position_in_imu,
 )
 
-optical_axes_in_imu = transform_scene_to_imu(
-    optical_axes.T,
-)
-optical_axes_in_world = transform_imu_to_world(
-    optical_axes_in_imu.T, imu_quaternions,
+# The optical axes are unit vectors originating at the eyeball centers,
+# so there is no need to translate them.
+optical_axes_in_world = transform_scene_to_world(
+    optical_axes, imu_quaternions,
 )
 ```