Synchronizing gaze data stream to video stream

1 files changed, 22 insertions, 10 deletions

diff --git a/src/argaze/utils/live_tobii_session.py b/src/argaze/utils/live_tobii_session.py
index 6c58d10..db3d44e 100644
--- a/src/argaze/utils/live_tobii_session.py
+++ b/src/argaze/utils/live_tobii_session.py
@@ -3,7 +3,7 @@
 import argparse
 import os, time
 
-from argaze import GazeFeatures
+from argaze import GazeFeatures, DataStructures
 from argaze.TobiiGlassesPro2 import *
 
 import cv2 as cv
@@ -11,7 +11,7 @@ import numpy
 
 def main():
     """
-    Capture video camera and display gaze point
+    Capture video camera and gaze data streams and synchronise them.
     """
 
     # Manage arguments
@@ -38,27 +38,39 @@ def main():
     # Live video stream capture loop
     try:
 
+        past_gaze_positions = DataStructures.TimeStampedBuffer()
+
         while tobii_video_stream.is_alive():
 
             video_ts, video_frame = tobii_video_stream.read()
                 
             try:
 
-                # read data stream
+                # Read data stream
                 data_stream = tobii_data_stream.read()
 
-                # get last gaze position
-                last_ts, last_gaze_position = data_stream.gidx_l_gp.pop_last()
+                # Store received gaze positions
+                for ts in list(data_stream.gidx_l_gp.keys()):
+                    past_gaze_positions[ts] = data_stream.gidx_l_gp.pop(ts)
+
+                # Get last gaze position timestamp before video timestamp
+                earliest_ts = past_gaze_positions.get_last_before(video_ts)
+
+                # When no timestamped gaze position have been found
+                if earliest_ts == None:
+                    raise ValueError
+
+                earliest_ts, earliest_gaze_position = past_gaze_positions.pop_first_until(earliest_ts)
 
-                # Draw tobii gaze pointer
-                pointer = (int(last_gaze_position.gp[0] * video_frame.width), int(last_gaze_position.gp[1] * video_frame.height))
+                # Draw video synchronized gaze pointer
+                pointer = (int(earliest_gaze_position.gp[0] * video_frame.width), int(earliest_gaze_position.gp[1] * video_frame.height))
                 cv.circle(video_frame.matrix, pointer, 4, (0, 255, 255), -1)
 
-            # when gidx_l_gp key not in data stream
-            except (KeyError, AttributeError):
+            # When expected values aren't in data stream
+            except (KeyError, AttributeError, ValueError):
                 pass
             
-            # close window using 'Esc' key
+            # Close window using 'Esc' key
             if cv.waitKey(1) == 27:
                 break