Refactoring warnings. Separating video frame processing and visualisations. Improving aruco tracking into focus area only. Making focus area rectangular. Ignoring frame where hed is moving.

1 files changed, 133 insertions, 87 deletions

diff --git a/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py b/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
index 4825c9e..653a5fa 100644
--- a/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
+++ b/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
@@ -95,6 +95,9 @@ def main():
         # Access to timestamped gaze 3D positions data buffer
         tobii_ts_gaze_positions_3d = tobii_segment_data['GazePosition3D']
 
+        # Access to timestamped head rotations data buffer
+        tobii_ts_head_rotations = tobii_segment_data['Gyroscope']
+
         # Prepare video exportation at the same format than segment video
         output_video = TobiiVideo.TobiiVideoOutput(vs_video_filepath, tobii_segment_video.get_stream())
 
@@ -199,140 +202,183 @@ def main():
             # Initialise progress bar
             #MiscFeatures.printProgressBar(0, tobii_segment_video.get_duration()/1000, prefix = 'Progress:', suffix = 'Complete', length = 100)
 
+            head_moving = False
+            head_movement_last = 0.
+
             # Iterate on video frames
             for video_ts, video_frame in tobii_segment_video.frames():
 
                 video_ts_ms = video_ts / 1000
-
-                # Track markers with pose estimation and draw them
-                aruco_tracker.track(video_frame.matrix)
-                aruco_tracker.draw(video_frame.matrix)
+                visu_frame = video_frame.copy()
 
                 # Write segment timing
-                cv.putText(video_frame.matrix, f'Segment time: {int(video_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
-
+                cv.putText(visu_frame.matrix, f'Segment time: {int(video_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (127, 127, 127), 1, cv.LINE_AA)
+               
+                # Draw focus area
+                cv.rectangle(visu_frame.matrix, (int(video_frame.width/6), 0), (int(video_frame.width*(1-1/6)), int(video_frame.height)), (255, 150, 150), 1)
+                       
+                # Draw center
+                cv.line(visu_frame.matrix, (int(visu_frame.width/2) - 50, int(visu_frame.height/2)), (int(visu_frame.width/2) + 50, int(visu_frame.height/2)), (255, 150, 150), 1)
+                cv.line(visu_frame.matrix, (int(visu_frame.width/2), int(visu_frame.height/2) - 50), (int(visu_frame.width/2), int(visu_frame.height/2) + 50), (255, 150, 150), 1)
+                
+                # Process video and data frame
                 try:
 
+                    # Get nearest head rotation before video timestamp and remove all head rotations before
+                    _, nearest_head_rotation = tobii_ts_head_rotations.pop_first_until(video_ts)
+
+                    # Calculate head movement considering only head yaw and pitch
+                    head_movement = numpy.array(nearest_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(visu_frame.matrix, (int(visu_frame.width/2), int(visu_frame.height/2)), (int(visu_frame.width/2) + head_movement_px[1], int(visu_frame.height/2) - head_movement_px[0]), (150, 150, 150), 3)
+                
+                    # Head movement detection hysteresis
+                    # TODO : pass the threshold value as argument
+                    if not head_moving and head_movement_norm > 50:
+                        head_moving = True
+                        
+                    if head_moving and head_movement_norm < 10:
+                        head_moving = False
+
+                    # Ignore frame where head is moving
+                    if head_moving:
+                        raise UserWarning('Head is moving')
+
                     # Get nearest gaze position before video timestamp and remove all gaze positions before
                     _, nearest_gaze_position = tobii_ts_gaze_positions.pop_first_until(video_ts)
 
+                    gaze_position_pixel = (int(nearest_gaze_position.value[0] * visu_frame.width), int(nearest_gaze_position.value[1] * visu_frame.height))
+
+                    # Draw gaze position
+                    cv.circle(visu_frame.matrix, gaze_position_pixel, 2, (0, 255, 255), -1)
+
                     # Get nearest gaze position 3D before video timestamp and remove all gaze positions before
                     _, nearest_gaze_position_3d = tobii_ts_gaze_positions_3d.pop_first_until(video_ts)
 
-                    # Consider gaze position if gaze precision can be evaluated
-                    if nearest_gaze_position_3d.value[2] > 0:
+                    # Ignore frame when gaze precison can't be evaluated
+                    if nearest_gaze_position_3d.value[2] <= 0:
+                        raise UserWarning('Negative Z gaze position 3D value')
 
-                        gaze_position_pixel = (int(nearest_gaze_position.value[0] * video_frame.width), int(nearest_gaze_position.value[1] * video_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_pixel = round(visu_frame.width * float(gaze_accuracy_mm) / float(tobii_camera_hfov_mm))
 
-                        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_pixel = round(video_frame.width * float(gaze_accuracy_mm) / float(tobii_camera_hfov_mm))
+                    # Draw gaze accuracy
+                    cv.circle(visu_frame.matrix, gaze_position_pixel, gaze_accuracy_pixel, (0, 255, 255), 1)
 
-                        # Draw gaze position and accuracy
-                        cv.circle(video_frame.matrix, gaze_position_pixel, 2, (0, 255, 255), -1)
-                        cv.circle(video_frame.matrix, gaze_position_pixel, gaze_accuracy_pixel, (0, 255, 255), 1)
+                    # Store gaze position and precision at this time in millisecond
+                    ts_gaze_positions[round(video_ts_ms)] = gaze_position_pixel
+                    ts_gaze_accuracies[round(video_ts_ms)] = gaze_accuracy_pixel
 
-                        # Store gaze position and precision at this time in millisecond
-                        ts_gaze_positions[round(video_ts_ms)] = gaze_position_pixel
-                        ts_gaze_accuracies[round(video_ts_ms)] = gaze_accuracy_pixel
+                    # Hide frame left and right borders before tracking to ignore markers outside focus area
+                    cv.rectangle(video_frame.matrix, (0, 0), (int(video_frame.width/6), int(video_frame.height)), (0, 0, 0), -1)
+                    cv.rectangle(video_frame.matrix, (int(video_frame.width*(1 - 1/6)), 0), (int(video_frame.width), int(video_frame.height)), (0, 0, 0), -1)
 
-                    else:
+                    # Track markers with pose estimation and draw them
+                    aruco_tracker.track(video_frame.matrix)
+                    aruco_tracker.draw(visu_frame.matrix)
 
-                        ValueError('Unable to evaluate gaze precision')
+                    # Project 3D scene on each video frame and the visualisation frame
+                    if aruco_tracker.get_markers_number():
 
-                # Wait for gaze position
-                except ValueError:
-                    continue
+                        # Store aoi 2D video for further scene merging
+                        aoi2D_dict = {}
 
-                # Draw focus area
-                cv.circle(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2)), int(video_frame.width/3), (255, 150, 150), 1)
-                        
-                # Draw focus area 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)
-                 
-                # Project 3D scene on each video frame and the visualisation frame
-                if aruco_tracker.get_markers_number():
+                        for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()):
 
-                    # Store aoi 2D video for further scene merging
-                    aoi2D_dict = {}
+                            # Process marker pose
+                            try:
 
-                    for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()):
+                                # Copy 3D scene related to detected marker
+                                aoi3D_scene = aoi3D_scene_selector(marker_id)
+                                
+                                if aoi3D_scene == None:
+                                    raise UserWarning('No AOI 3D scene')
 
-                        # Copy 3D scene related to detected marker
-                        aoi3D_scene = aoi3D_scene_selector(marker_id)
-                        
-                        if aoi3D_scene == None:
-                            continue
-                        
-                        # Ignore marker out of focus area
-                        marker_x, marker_y = aruco_tracker.get_marker_center(i)
-                        distance_to_center = ( (video_frame.width/2 - marker_x)**2 + (video_frame.height/2 - marker_y)**2 )**0.5
+                                # Transform scene into camera referential
+                                aoi3D_camera = aoi3D_scene.transform(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i))
 
-                        if distance_to_center > int(video_frame.width/3):
-                            continue
+                                # Get aoi inside vision cone field 
+                                cone_vision_height_cm = nearest_gaze_position_3d.value[2]/10 # cm
+                                cone_vision_radius_cm = numpy.tan(numpy.deg2rad(tobii_visual_hfov / 2)) * cone_vision_height_cm
 
-                        # Transform scene into camera referential
-                        aoi3D_camera = aoi3D_scene.transform(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i))
+                                aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(cone_vision_radius_cm, cone_vision_height_cm)
 
-                        # Get aoi inside vision cone field 
-                        cone_vision_height_cm = nearest_gaze_position_3d.value[2]/10 # cm
-                        cone_vision_radius_cm = numpy.tan(numpy.deg2rad(tobii_visual_hfov / 2)) * cone_vision_height_cm
+                                # Keep only aoi inside vision cone field
+                                aoi3D_scene = aoi3D_scene.copy(exclude=aoi3D_outside.keys())
 
-                        aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(cone_vision_radius_cm, cone_vision_height_cm)
+                                # DON'T APPLY CAMERA DISTORSION : it projects points which are far from the frame into it
+                                # This hack isn't realistic but as the gaze will mainly focus on centered AOI, where the distorsion is low, it is acceptable.
+                                aoi2D_video_scene = aoi3D_scene.project(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i), aruco_camera.get_K())
 
-                        # Keep only aoi inside vision cone field
-                        aoi3D_scene = aoi3D_scene.copy(exclude=aoi3D_outside.keys())
+                                # Store each 2D aoi for further scene merging
+                                for name, aoi in aoi2D_video_scene.items():
 
-                        # DON'T APPLY CAMERA DISTORSION : it projects points which are far from the frame into it
-                        # This hack isn't realistic but as the gaze will mainly focus on centered AOI, where the distorsion is low, it is acceptable.
-                        aoi2D_video_scene = aoi3D_scene.project(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i), aruco_camera.get_K())
+                                    if name not in aoi2D_dict.keys():
+                                        aoi2D_dict[name] = []
 
-                        # Store each 2D aoi for further scene merging
-                        for name, aoi in aoi2D_video_scene.items():
+                                    aoi2D_dict[name].append(aoi.clockwise())
 
-                            if name not in aoi2D_dict.keys():
-                                aoi2D_dict[name] = []
+                                # Select 2D visu scene if there is one for the detected marker
+                                aoi2D_visu_scene = aoi2D_visu_scene_selector(marker_id)
+                                aoi2D_visu_frame = aoi2D_visu_frame_selector(marker_id)
+                                
+                                if aoi2D_visu_scene == None:
+                                    continue
+                                
+                                look_at = aoi2D_video_scene['Visualisation_Plan'].look_at(gaze_position_pixel)
 
-                            aoi2D_dict[name].append(aoi.clockwise())
+                                visu_gaze_pixel = aoi2D_visu_scene['Visualisation_Plan'].looked_pixel(look_at)
+                                cv.circle(aoi2D_visu_frame, visu_gaze_pixel, 4, (0, 0, 255), -1)
 
-                        # Select 2D visu scene if there is one for the detected marker
-                        aoi2D_visu_scene = aoi2D_visu_scene_selector(marker_id)
-                        aoi2D_visu_frame = aoi2D_visu_frame_selector(marker_id)
-                        
-                        if aoi2D_visu_scene == None:
-                            continue
-                        
-                        look_at = aoi2D_video_scene['Visualisation_Plan'].look_at(gaze_position_pixel)
+                            # Write warning related to marker pose processing 
+                            except UserWarning as e:
+                    
+                                cv.putText(visu_frame.matrix, f'Marker {marker_id}: {e}', (20, int(visu_frame.height) - (marker_id+1) * 40), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
 
-                        visu_gaze_pixel = aoi2D_visu_scene['Visualisation_Plan'].looked_pixel(look_at)
-                        cv.circle(aoi2D_visu_frame, visu_gaze_pixel, 4, (0, 0, 255), -1)
+                        # Merge all 2D aoi into a single 2D scene
+                        aoi2D_merged_scene = AOI2DScene.AOI2DScene()
+                        for name, aoi_array in aoi2D_dict.items():
+                            aoi2D_merged_scene[name] = numpy.sum(aoi_array, axis=0) / len(aoi_array)
 
-                    # Merge all 2D aoi into a single 2D scene
-                    aoi2D_merged_scene = AOI2DScene.AOI2DScene()
-                    for name, aoi_array in aoi2D_dict.items():
-                        aoi2D_merged_scene[name] = numpy.sum(aoi_array, axis=0) / len(aoi_array)
+                        aoi2D_merged_scene.draw(visu_frame.matrix, gaze_position_pixel, gaze_accuracy_pixel, exclude=['Visualisation_Plan'])
+                            
+                        # Store 2D merged scene at this time in millisecond
+                        ts_aois_scenes[round(video_ts_ms)] = aoi2D_merged_scene
 
-                    aoi2D_merged_scene.draw(video_frame.matrix, gaze_position_pixel, gaze_accuracy_pixel, exclude=['Visualisation_Plan'])
-                        
-                    # Store 2D merged scene at this time in millisecond
-                    ts_aois_scenes[round(video_ts_ms)] = aoi2D_merged_scene
+                    else:
+
+                        raise UserWarning('No marker detected')
 
+                # Write warning related to video and data frame processing
+                except UserWarning as e:
+                    
+                    cv.putText(visu_frame.matrix, str(e), (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
+
+                except ValueError:
+                    pass
+               
                 if args.window:
 
                     # Close window using 'Esc' key
                     if cv.waitKey(1) == 27:
                         break
 
-                    # Display video
-                    cv.imshow(f'Segment {tobii_segment.get_id()} ArUco AOI', video_frame.matrix)
+                    # Display video frame
+                    cv.imshow(f'Segment {tobii_segment.get_id()}', video_frame.matrix)
+
+                    # Display visualisation
+                    cv.imshow(f'Segment {tobii_segment.get_id()} ArUco AOI', visu_frame.matrix)
 
                     # Display each visual scan frame
-                    for marker_id, visu_frame in aoi2D_visu_frames.items():
-                        cv.imshow(f'Segment {tobii_segment.get_id()} visual scan for marker {marker_id}', visu_frame)
+                    for marker_id, aoi2D_visu_frame in aoi2D_visu_frames.items():
+                        cv.imshow(f'Segment {tobii_segment.get_id()} visual scan for marker {marker_id}', visu_frame.matrix)
 
                 # Write video
-                output_video.write(video_frame.matrix)
+                output_video.write(visu_frame.matrix)
 
                 # Update Progress Bar
                 progress = video_ts_ms - int(args.time_range[0] * 1000)
@@ -367,8 +413,8 @@ def main():
         print(f'Visual scan data saved into {vs_data_filepath}')
 
         # Export each visual scan picture
-        for marker_id, visu_frame in aoi2D_visu_frames.items():
-            cv.imwrite(vs_visu_filepath % marker_id, visu_frame)
+        for marker_id, aoi2D_visu_frame in aoi2D_visu_frames.items():
+            cv.imwrite(vs_visu_filepath % marker_id, visu_frame.matrix)
             print(f'Visual scan picture for marker {marker_id} saved into {vs_visu_filepath % marker_id}')
 
         # Notify when the visual scan video has been exported