Returning last streamed data timestamp.

4 files changed, 135 insertions, 42 deletions

diff --git a/src/argaze/TobiiGlassesPro2/TobiiData.py b/src/argaze/TobiiGlassesPro2/TobiiData.py
index 293f460..975afce 100644
--- a/src/argaze/TobiiGlassesPro2/TobiiData.py
+++ b/src/argaze/TobiiGlassesPro2/TobiiData.py
@@ -397,7 +397,7 @@ class TobiiDataStream(threading.Thread):
                 self.__first_ts = ts
             
             ts -= self.__first_ts
-            
+
             # ignore negative timestamp
             if ts < 0:
                 break
@@ -412,4 +412,4 @@ class TobiiDataStream(threading.Thread):
         # unlock data queue access
         self.__read_lock.release()
 
-        return ts_data_buffer_dict
+        return ts, ts_data_buffer_dict
diff --git a/src/argaze/utils/tobii_stream_aruco_aoi_display.py b/src/argaze/utils/tobii_stream_aruco_aoi_display.py
index 6391a0d..1b7ab2d 100644
--- a/src/argaze/utils/tobii_stream_aruco_aoi_display.py
+++ b/src/argaze/utils/tobii_stream_aruco_aoi_display.py
@@ -16,25 +16,26 @@ from ivy.std_api import *
 
 def main():
     """
-    Track any ArUco marker into Tobii Glasses Pro 2 camera video stream. 
+    Track any ArUco marker into Tobii Glasses Pro 2 camera video stream.
+    For each loaded AOI scene .obj file, position the scene virtually relatively to each detected ArUco markers and project the scene into camera frame. 
     """
 
     # Manage arguments
     parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
     parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default='192.168.1.10', help='tobii glasses ip')
-    parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default='tobii_camera.json', help='json camera calibration filepath')
-    parser.add_argument('-d', '--dictionary', metavar='DICT', type=str, default='DICT_ARUCO_ORIGINAL', help='aruco marker dictionnary (DICT_4X4_50, DICT_4X4_100, DICT_4X4_250, DICT_4X4_1000, DICT_5X5_50, DICT_5X5_100, DICT_5X5_250, DICT_5X5_1000, DICT_6X6_50, DICT_6X6_100, DICT_6X6_250, DICT_6X6_1000, DICT_7X7_50, DICT_7X7_100, DICT_7X7_250, DICT_7X7_1000, DICT_ARUCO_ORIGINAL,DICT_APRILTAG_16h5, DICT_APRILTAG_25h9, DICT_APRILTAG_36h10, DICT_APRILTAG_36h11)')
-    parser.add_argument('-m', '--marker_size', metavar='MKR', type=float, default=6, help='aruco marker size (cm)')
+    parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default=None, help='json camera calibration filepath')
+    parser.add_argument('-p', '--aruco_tracker_configuration', metavar='TRACK_CONFIG', type=str, default=None, help='json aruco tracker configuration filepath')
+    parser.add_argument('-md', '--marker_dictionary', metavar='MARKER_DICT', type=str, default='DICT_ARUCO_ORIGINAL', help='aruco marker dictionnary (DICT_4X4_50, DICT_4X4_100, DICT_4X4_250, DICT_4X4_1000, DICT_5X5_50, DICT_5X5_100, DICT_5X5_250, DICT_5X5_1000, DICT_6X6_50, DICT_6X6_100, DICT_6X6_250, DICT_6X6_1000, DICT_7X7_50, DICT_7X7_100, DICT_7X7_250, DICT_7X7_1000, DICT_ARUCO_ORIGINAL,DICT_APRILTAG_16h5, DICT_APRILTAG_25h9, DICT_APRILTAG_36h10, DICT_APRILTAG_36h11)')
+    parser.add_argument('-ms', '--marker_size', metavar='MARKER_SIZE', type=float, default=6, help='aruco marker size (cm)')
+    parser.add_argument('-mi', '--marker_id_scene', metavar='MARKER_ID_SCENE', type=json.loads, help='{"marker": "aoi scene filepath"} dictionary')
+    parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction)
     args = parser.parse_args()
 
-    print(f'Track Aruco markers from the {args.dictionary} dictionary')
+    print(f'Track any Aruco markers from the {args.marker_dictionary} dictionary')
 
     # Create tobii controller
     tobii_controller = TobiiController.TobiiController(args.tobii_ip, 'myProject', 'mySelf')
 
-    # Calibrate tobii glasses
-    tobii_controller.calibrate()
-
     # Enable tobii data stream
     tobii_data_stream = tobii_controller.enable_data_stream()
 
@@ -43,10 +44,49 @@ def main():
 
     # create aruco camera
     aruco_camera = ArUcoCamera.ArUcoCamera()
-    aruco_camera.load_calibration_file(args.camera_calibration)
+    
+     # Load calibration file
+    if args.camera_calibration != None:
+
+        aruco_camera.load_calibration_file(args.camera_calibration)
+
+    else:
+
+        raise UserWarning('.json camera calibration filepath required. Use -c option.')
 
     # Create aruco tracker
-    aruco_tracker = ArUcoTracker.ArUcoTracker(args.dictionary, args.marker_size, aruco_camera)
+    aruco_tracker = ArUcoTracker.ArUcoTracker(args.marker_dictionary, args.marker_size, aruco_camera)
+
+    # Load specific configuration file
+    if args.aruco_tracker_configuration != None:
+
+        aruco_tracker.load_configuration_file(args.aruco_tracker_configuration)
+
+        print(f'ArUcoTracker configuration for {aruco_tracker.get_markers_dictionay().get_markers_format()} markers detection:')
+        aruco_tracker.print_configuration()
+
+    # Load AOI 3D scene for each marker and create a AOI 2D scene and frame when a 'Visualisation_Plan' AOI exist
+    aoi3D_scenes = {}
+    aoi2D_visu_scenes = {}
+    aoi2D_visu_frames = {}
+
+    for marker_id, aoi_scene_filepath in args.marker_id_scene.items():
+
+        marker_id = int(marker_id)
+        
+        aoi3D_scenes[marker_id] = AOI3DScene.AOI3DScene()
+        aoi3D_scenes[marker_id].load(aoi_scene_filepath)
+
+        print(f'AOI in {os.path.basename(aoi_scene_filepath)} scene related to marker #{marker_id}:')
+        for aoi in aoi3D_scenes[marker_id].keys():
+
+            print(f'\t{aoi}')
+
+    def aoi3D_scene_selector(marker_id):
+        return aoi3D_scenes.get(marker_id, None)
+
+    # Create timestamped buffer to store AOIs scene in time
+    ts_aois_scenes = AOIFeatures.TimeStampedAOIScenes()
 
     # Start streaming
     tobii_controller.start_streaming()
diff --git a/src/argaze/utils/tobii_stream_aruco_aoi_ivy_controller.py b/src/argaze/utils/tobii_stream_aruco_aoi_ivy_controller.py
index 070e3ee..31b5969 100644
--- a/src/argaze/utils/tobii_stream_aruco_aoi_ivy_controller.py
+++ b/src/argaze/utils/tobii_stream_aruco_aoi_ivy_controller.py
@@ -79,13 +79,6 @@ def main():
     def aoi3D_scene_selector(marker_id):
         return aoi3D_scenes.get(marker_id, None)
 
-    # !!! the parameters below are specific to the TobiiGlassesPro2 !!!
-    # Reference : https://www.biorxiv.org/content/10.1101/299925v1
-    tobii_accuracy = 1.42 # degree
-    tobii_precision = 0.34 # degree
-    tobii_camera_hfov = 82 # degree
-    tobii_visual_hfov = 160 # degree
-
     # Start streaming
     tobii_controller.start_streaming()
    
@@ -159,8 +152,8 @@ def main():
 
                 gaze_position_pixel = (int(nearest_gaze_position.value[0] * visu_frame.width), int(nearest_gaze_position.value[1] * visu_frame.height))
 
-                gaze_accuracy_mm = numpy.tan(numpy.deg2rad(tobii_accuracy)) * nearest_gaze_position_3d.value[2]
-                tobii_camera_hfov_mm = numpy.tan(numpy.deg2rad(tobii_camera_hfov / 2)) * nearest_gaze_position_3d.value[2]
+                gaze_accuracy_mm = numpy.tan(numpy.deg2rad(TobiiSpecifications.ACCURACY)) * nearest_gaze_position_3d.value[2]
+                tobii_camera_hfov_mm = numpy.tan(numpy.deg2rad(TobiiSpecifications.CAMERA_HFOV / 2)) * nearest_gaze_position_3d.value[2]
                 
                 gaze_position_pixel.accuracy = round(visu_frame.width * float(gaze_accuracy_mm) / float(tobii_camera_hfov_mm))
 
@@ -251,7 +244,7 @@ def main():
                 cv.putText(visu_frame.matrix, str(e), (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
 
             # Raised when buffer is empty
-            except ValueError:
+            except KeyError:
                 pass
 
             # Draw focus area
diff --git a/src/argaze/utils/tobii_stream_display.py b/src/argaze/utils/tobii_stream_display.py
index b849357..76da3d6 100644
--- a/src/argaze/utils/tobii_stream_display.py
+++ b/src/argaze/utils/tobii_stream_display.py
@@ -23,9 +23,6 @@ def main():
     # Create tobii controller
     tobii_controller = TobiiController.TobiiController(args.tobii_ip, 'myProject', 'mySelf')
 
-    # Calibrate tobii glasses
-    tobii_controller.calibrate()
-
     # Enable tobii data stream
     tobii_data_stream = tobii_controller.enable_data_stream()
 
@@ -35,39 +32,102 @@ def main():
     # Start streaming
     tobii_controller.start_streaming()
 
-    # Live video stream capture loop
-    try:
+    # Prepare to timestamped gaze position data stream bufferring
+    tobii_ts_gaze_positions = DataStructures.TimeStampedBuffer()
+
+    # Prepare to timestamped gaze position 3d data stream bufferring
+    tobii_ts_gaze_positions_3d = DataStructures.TimeStampedBuffer()
 
-        past_gaze_positions = DataStructures.TimeStampedBuffer()
+    # Prepare to timestamped head rotations data stream bufferring
+    tobii_ts_head_rotations = DataStructures.TimeStampedBuffer()
+
+    # Live video and data stream capture loop
+    try:
 
         while tobii_video_stream.is_alive():
 
+            # Read video stream
             video_ts, video_frame = tobii_video_stream.read()
-                
+            video_ts_ms = video_ts / 1e3
+
+            # Read data stream
+            data_ts, data_stream = tobii_data_stream.read()
+            data_ts_ms = data_ts / 1e3
+
             try:
 
-                # Read data stream
-                data_stream = tobii_data_stream.read()
+                # Buffer last received gaze positions
+                tobii_ts_gaze_positions.append(data_stream['GazePosition'])
 
-                # Store received gaze positions
-                past_gaze_positions.append(data_stream['GazePosition'])
+                # Buffer last received gaze positions 3d
+                tobii_ts_gaze_positions_3d.append(data_stream['GazePosition3D'])
 
-                # Get last gaze position before video timestamp and remove all former gaze positions
-                earliest_ts, earliest_gaze_position = past_gaze_positions.pop_first_until(video_ts)
+                # Buffer last received gaze positions 3d
+                tobii_ts_head_rotations.append(data_stream['Gyroscope'])
 
-                # Draw gaze position
-                video_gaze_pixel = (int(earliest_gaze_position.value[0] * video_frame.width), int(earliest_gaze_position.value[1] * video_frame.height))
-                cv.circle(video_frame.matrix, video_gaze_pixel, 4, (0, 255, 255), -1)
+            # Ignore missing data stream
+            except KeyError as e:
+                pass
 
-            # Wait for gaze position
-            except (AttributeError, ValueError):
-                continue
+            try:
+
+                # Get nearest head rotation before video timestamp and remove all head rotations before
+                earliest_ts, earliest_head_rotation = tobii_ts_head_rotations.pop_last()
+
+                # Calculate head movement considering only head yaw and pitch
+                head_movement = numpy.array(earliest_head_rotation.value)
+                head_movement_px = head_movement.astype(int)
+                head_movement_norm = numpy.linalg.norm(head_movement[0:2])
+
+                # Draw movement vector
+                cv.line(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2)), (int(video_frame.width/2) + head_movement_px[1], int(video_frame.height/2) - head_movement_px[0]), (150, 150, 150), 3)
             
+            # Wait for head rotation
+            except KeyError:
+                pass
+
+            try:
+
+                # Get nearest gaze position before video timestamp and remove all gaze positions before
+                _, earliest_gaze_position = tobii_ts_gaze_positions.pop_last()
+
+                # Ignore frame when gaze position is not valid
+                if earliest_gaze_position.validity == 0:
+                
+                    gaze_position_pixel = GazeFeatures.GazePosition( (int(earliest_gaze_position.value[0] * video_frame.width), int(earliest_gaze_position.value[1] * video_frame.height)) )
+                    
+                    # Get nearest gaze position 3D before video timestamp and remove all gaze positions before
+                    _, earliest_gaze_position_3d = tobii_ts_gaze_positions_3d.pop_last()
+
+                    # Ignore frame when gaze position 3D is not valid
+                    if earliest_gaze_position_3d.validity == 0:
+                        
+                        gaze_accuracy_mm = numpy.tan(numpy.deg2rad(TobiiSpecifications.ACCURACY)) * earliest_gaze_position_3d.value[2]
+                        tobii_camera_hfov_mm = numpy.tan(numpy.deg2rad(TobiiSpecifications.CAMERA_HFOV / 2)) * earliest_gaze_position_3d.value[2]
+
+                        gaze_position_pixel.accuracy = round(video_frame.width * float(gaze_accuracy_mm) / float(tobii_camera_hfov_mm))
+                    
+                        # Draw gaze
+                        gaze_position_pixel.draw(video_frame.matrix)
+
+            # Wait for gaze position
+            except KeyError:
+                pass
+
+            # Draw center
+            cv.line(video_frame.matrix, (int(video_frame.width/2) - 50, int(video_frame.height/2)), (int(video_frame.width/2) + 50, int(video_frame.height/2)), (255, 150, 150), 1)
+            cv.line(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2) - 50), (int(video_frame.width/2), int(video_frame.height/2) + 50), (255, 150, 150), 1)
+
+            # Write stream timing
+            cv.rectangle(video_frame.matrix, (0, 0), (950, 50), (63, 63, 63), -1)
+            cv.putText(video_frame.matrix, f'Data stream time: {int(data_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
+            cv.putText(video_frame.matrix, f'Video delay: {int(data_ts_ms - video_ts_ms)} ms', (550, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
+
             # Close window using 'Esc' key
             if cv.waitKey(1) == 27:
                 break
 
-            cv.imshow(f'Live Tobii Camera', video_frame.matrix)
+            cv.imshow(f'Video and data stream', video_frame.matrix)
 
     # Exit on 'ctrl+C' interruption
     except KeyboardInterrupt: