diff options
| -rw-r--r-- | src/argaze/utils/edit_tobii_segment_aruco_pose.py | 161 | ||||
| -rw-r--r-- | src/argaze/utils/export_tobii_segment_aruco_visual_scan.py | 112 | ||||
| -rw-r--r-- | src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py | 160 |
3 files changed, 242 insertions, 191 deletions
diff --git a/src/argaze/utils/edit_tobii_segment_aruco_pose.py b/src/argaze/utils/edit_tobii_segment_aruco_pose.py index 238a0b7..72232fe 100644 --- a/src/argaze/utils/edit_tobii_segment_aruco_pose.py +++ b/src/argaze/utils/edit_tobii_segment_aruco_pose.py @@ -178,7 +178,7 @@ def main(): # Frame selector loop frame_index = 0 last_frame_index = -1 - last_frame_matrix = video_frame.matrix.copy() + last_frame = video_frame.copy() selected_marker_id = -1 @@ -193,50 +193,60 @@ def main(): video_ts_ms = video_ts / 1000 last_frame_index = frame_index - last_frame_matrix = video_frame.matrix.copy() + last_frame = video_frame.copy() + + # 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) + + # Track markers with pose estimation + aruco_tracker.track(video_frame.matrix) else: - video_frame.matrix = last_frame_matrix.copy() + video_frame = last_frame.copy() + + # Copy video frame to edit visualisation on it with out disrupting aruco tracking + visu_frame = video_frame.copy() - # Track markers with pose estimation and draw them - aruco_tracker.track(video_frame.matrix) - aruco_tracker.draw(video_frame.matrix) + # Draw markers and pose estimation + aruco_tracker.draw(visu_frame.matrix) # 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.rectangle(visu_frame.matrix, (0, 0), (550, 50), (63, 63, 63), -1) + cv.putText(visu_frame.matrix, f'Segment time: {int(video_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) # 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) + cv.rectangle(visu_frame.matrix, (int(visu_frame.width/6), 0), (int(visu_frame.width*(1-1/6)), int(visu_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) # Draw pointer - cv.circle(video_frame.matrix, pointer, 2, (0, 255, 255), -1) + cv.circle(visu_frame.matrix, pointer, 2, (0, 255, 255), -1) # Write selected marker id if selected_marker_id >= 0: if edit_trans: - cv.putText(video_frame.matrix, f'Marker {selected_marker_id} R Axis {edit_coord + 1} selected', (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Marker {selected_marker_id}: R Axis {edit_coord + 1} selected', (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) else: - cv.putText(video_frame.matrix, f'Marker {selected_marker_id} T Axis {edit_coord + 1} selected', (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Marker {selected_marker_id}: T Axis {edit_coord + 1} selected', (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) # Write documentation else: - cv.putText(video_frame.matrix, f'Left click on marker to select scene', (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'T to translate, R to rotate', (20, 120), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'Shift+num to select axis', (20, 160), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'Right click and drag to edit axis', (20, 200), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'Ctrl+s to save scene', (20, 240), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Left click on marker to select scene', (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'T to translate, R to rotate', (20, 120), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Shift+num to select axis', (20, 160), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Right click and drag to edit axis', (20, 200), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Ctrl+s to save scene', (20, 240), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) # Project 3D scene on each video frame and the visualisation frame if aruco_tracker.get_markers_number(): # Write detected marker ids - cv.putText(video_frame.matrix, f'Detected markers : {aruco_tracker.get_markers_ids()}', (20, video_frame.height - 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Detected markers: {aruco_tracker.get_markers_ids()}', (20, visu_frame.height - 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) # Update selected marker id by left_clicking on marker for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()): @@ -256,86 +266,79 @@ def main(): # Retreive marker index selected_marker_index = aruco_tracker.get_marker_index(selected_marker_id) - # If AOI scene is found - if aoi3D_scene != None: - - # Is the marker out of focus area ? - marker_x, marker_y = aruco_tracker.get_marker_center(selected_marker_index) - distance_to_center = ( (video_frame.width/2 - marker_x)**2 + (video_frame.height/2 - marker_y)**2 )**0.5 + if aoi3D_scene == None: + raise UserWarning('No AOI 3D scene') - if distance_to_center > int(video_frame.width/3): + # Select scene edit + aoi3D_scene_edit = aoi3D_scene_edit_selector(selected_marker_id) - # Write warning - cv.putText(video_frame.matrix, f'Out of focus area', (20, 120), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) + # Edit scene + if aoi3D_scene_edit != None: - # Select scene edit - aoi3D_scene_edit = aoi3D_scene_edit_selector(selected_marker_id) - - # Edit scene - if aoi3D_scene_edit != None: + marker_x, marker_y = aruco_tracker.get_marker_center(selected_marker_index) - if right_button: + if right_button: - pointer_delta_x, pointer_delta_y = (right_click[0] - marker_x) / (video_frame.width/3), (marker_y - right_click[1]) / (video_frame.width/3) + pointer_delta_x, pointer_delta_y = (right_click[0] - marker_x) / (visu_frame.width/3), (marker_y - right_click[1]) / (visu_frame.width/3) - if edit_trans: - - # Edit scene rotation - if edit_coord == 0: - aoi3D_scene_edit['rotation'] = numpy.array([pointer_delta_y, aoi3D_scene_edit['rotation'][1], aoi3D_scene_edit['rotation'][2]]) + if edit_trans: + + # Edit scene rotation + if edit_coord == 0: + aoi3D_scene_edit['rotation'] = numpy.array([pointer_delta_y, aoi3D_scene_edit['rotation'][1], aoi3D_scene_edit['rotation'][2]]) - elif edit_coord == 1: - aoi3D_scene_edit['rotation'] = numpy.array([aoi3D_scene_edit['rotation'][0], pointer_delta_x, aoi3D_scene_edit['rotation'][2]]) + elif edit_coord == 1: + aoi3D_scene_edit['rotation'] = numpy.array([aoi3D_scene_edit['rotation'][0], pointer_delta_x, aoi3D_scene_edit['rotation'][2]]) - elif edit_coord == 2: - aoi3D_scene_edit['rotation'] = numpy.array([aoi3D_scene_edit['rotation'][0], aoi3D_scene_edit['rotation'][1], -1*pointer_delta_y]) + elif edit_coord == 2: + aoi3D_scene_edit['rotation'] = numpy.array([aoi3D_scene_edit['rotation'][0], aoi3D_scene_edit['rotation'][1], -1*pointer_delta_y]) - else: + else: - # Edit scene translation - if edit_coord == 0: - aoi3D_scene_edit['translation'] = numpy.array([pointer_delta_x, aoi3D_scene_edit['translation'][1], aoi3D_scene_edit['translation'][2]]) + # Edit scene translation + if edit_coord == 0: + aoi3D_scene_edit['translation'] = numpy.array([pointer_delta_x, aoi3D_scene_edit['translation'][1], aoi3D_scene_edit['translation'][2]]) - elif edit_coord == 1: - aoi3D_scene_edit['translation'] = numpy.array([aoi3D_scene_edit['translation'][0], pointer_delta_y, aoi3D_scene_edit['translation'][2]]) + elif edit_coord == 1: + aoi3D_scene_edit['translation'] = numpy.array([aoi3D_scene_edit['translation'][0], pointer_delta_y, aoi3D_scene_edit['translation'][2]]) - elif edit_coord == 2: - aoi3D_scene_edit['translation'] = numpy.array([aoi3D_scene_edit['translation'][0], aoi3D_scene_edit['translation'][1], 2*pointer_delta_y]) + elif edit_coord == 2: + aoi3D_scene_edit['translation'] = numpy.array([aoi3D_scene_edit['translation'][0], aoi3D_scene_edit['translation'][1], 2*pointer_delta_y]) - # Apply transformation - aoi3D_scene_edited = aoi3D_scene.transform(aoi3D_scene_edit['translation'], aoi3D_scene_edit['rotation']) + # Apply transformation + aoi3D_scene_edited = aoi3D_scene.transform(aoi3D_scene_edit['translation'], aoi3D_scene_edit['rotation']) - # Write rotation matrix - R, _ = cv.Rodrigues(aoi3D_scene_edit['rotation']) - cv.putText(video_frame.matrix, f'Rotation matrix:', (20, 160), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'{R[0][0]:.3f} {R[0][1]:.3f} {R[0][2]:.3f}', (40, 200), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'{R[1][0]:.3f} {R[1][1]:.3f} {R[1][2]:.3f}', (40, 240), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'{R[2][0]:.3f} {R[2][1]:.3f} {R[2][2]:.3f}', (40, 280), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) - - # Write translation vector - T = aoi3D_scene_edit['translation'] - cv.putText(video_frame.matrix, f'Translation vector:', (20, 320), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'{T[0]:.3f}', (40, 360), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'{T[1]:.3f}', (40, 400), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) - cv.putText(video_frame.matrix, f'{T[2]:.3f}', (40, 440), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) + # Write rotation matrix + R, _ = cv.Rodrigues(aoi3D_scene_edit['rotation']) + cv.putText(visu_frame.matrix, f'Rotation matrix:', (20, 160), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'{R[0][0]:.3f} {R[0][1]:.3f} {R[0][2]:.3f}', (40, 200), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'{R[1][0]:.3f} {R[1][1]:.3f} {R[1][2]:.3f}', (40, 240), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'{R[2][0]:.3f} {R[2][1]:.3f} {R[2][2]:.3f}', (40, 280), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) - # 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_edited.project(aruco_tracker.get_marker_translation(selected_marker_index), aruco_tracker.get_marker_rotation(selected_marker_index), aruco_camera.get_K()) + # Write translation vector + T = aoi3D_scene_edit['translation'] + cv.putText(visu_frame.matrix, f'Translation vector:', (20, 320), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'{T[0]:.3f}', (40, 360), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'{T[1]:.3f}', (40, 400), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'{T[2]:.3f}', (40, 440), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) + + # 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_edited.project(aruco_tracker.get_marker_translation(selected_marker_index), aruco_tracker.get_marker_rotation(selected_marker_index), aruco_camera.get_K()) - # Draw scene - aoi2D_video_scene.draw(video_frame.matrix, pointer, 2, exclude=['Visualisation_Plan']) + # Draw scene + aoi2D_video_scene.draw(visu_frame.matrix, pointer, 2, exclude=['Visualisation_Plan']) - else: + # Write warning related to marker pose processing + except UserWarning as e: - # Write error - cv.putText(video_frame.matrix, f'Marker {selected_marker_id} have no AOI scene', (20, 120), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Marker {selected_marker_id}: {e}', (20, 120), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) except ValueError: # Write error if selected_marker_id >= 0: - cv.putText(video_frame.matrix, f'Marker {selected_marker_id} not found', (20, 120), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) + cv.putText(visu_frame.matrix, f'Marker {selected_marker_id} not found', (20, 120), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) # Reset left_click left_click = (0, 0) @@ -390,7 +393,7 @@ def main(): load_configuration_file() # Display video - cv.imshow(f'Segment {tobii_segment.get_id()} ArUco marker editor', video_frame.matrix) + cv.imshow(f'Segment {tobii_segment.get_id()} ArUco marker editor', visu_frame.matrix) # Wait 1 second time.sleep(1) 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 18b932d..f8e1dbc 100644 --- a/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py +++ b/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py @@ -209,13 +209,16 @@ def main(): for video_ts, video_frame in tobii_segment_video.frames(): video_ts_ms = video_ts / 1000 + + # Copy video frame to edit visualisation on it without disrupting aruco tracking visu_frame = video_frame.copy() # Write segment timing - 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) + cv.rectangle(visu_frame.matrix, (0, 0), (550, 50), (63, 63, 63), -1) + cv.putText(visu_frame.matrix, f'Segment time: {int(video_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 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) + cv.rectangle(visu_frame.matrix, (int(video_frame.width/6), 0), (int(visu_frame.width*(1-1/6)), int(visu_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) @@ -281,77 +284,74 @@ def main(): aruco_tracker.track(video_frame.matrix) aruco_tracker.draw(visu_frame.matrix) - # Project 3D scene on each video frame and the visualisation frame - if aruco_tracker.get_markers_number(): - - # Store aoi 2D video for further scene merging - aoi2D_dict = {} - - for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()): + if aruco_tracker.get_markers_number() == 0: + raise UserWarning('No marker detected') - # Process marker pose - try: + # Store aoi 2D video for further scene merging + aoi2D_dict = {} - # Copy 3D scene related to detected marker - aoi3D_scene = aoi3D_scene_selector(marker_id) - - if aoi3D_scene == None: - raise UserWarning('No AOI 3D scene') + # Project 3D scene on each video frame and the visualisation frame + for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()): - # Transform scene into camera referential - aoi3D_camera = aoi3D_scene.transform(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i)) + # Process marker pose + try: - # 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 + # Copy 3D scene related to detected marker + aoi3D_scene = aoi3D_scene_selector(marker_id) + + if aoi3D_scene == None: + raise UserWarning('No AOI 3D scene') - aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(cone_vision_radius_cm, 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)) - # Keep only aoi inside vision cone field - aoi3D_scene = aoi3D_scene.copy(exclude=aoi3D_outside.keys()) + # 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 - # 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()) + aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(cone_vision_radius_cm, cone_vision_height_cm) - # Store each 2D aoi for further scene merging - for name, aoi in aoi2D_video_scene.items(): + # Keep only aoi inside vision cone field + aoi3D_scene = aoi3D_scene.copy(exclude=aoi3D_outside.keys()) - if name not in aoi2D_dict.keys(): - aoi2D_dict[name] = [] + # 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()) - aoi2D_dict[name].append(aoi.clockwise()) + # Store each 2D aoi for further scene merging + for name, aoi in aoi2D_video_scene.items(): - # 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) + if name not in aoi2D_dict.keys(): + aoi2D_dict[name] = [] - 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) + aoi2D_dict[name].append(aoi.clockwise()) - # 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) + # 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) - # 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) + 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) - 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 + # 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) - else: + # 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) - raise UserWarning('No marker detected') + 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 # Write warning related to video and data frame processing except UserWarning as e: diff --git a/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py b/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py index 1ab02e4..3fea974 100644 --- a/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py +++ b/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py @@ -93,108 +93,148 @@ def main(): try: past_gaze_positions = DataStructures.TimeStampedBuffer() + past_head_rotations = DataStructures.TimeStampedBuffer() + + head_moving = False + head_movement_last = 0. while tobii_video_stream.is_alive(): video_ts, video_frame = tobii_video_stream.read() - # Track markers with pose estimation and draw them - aruco_tracker.track(video_frame.matrix) - aruco_tracker.draw(video_frame.matrix) + # Copy video frame to edit visualisation on it without disrupting aruco tracking + visu_frame = video_frame.copy() + # Write stream timing + cv.rectangle(visu_frame.matrix, (0, 0), (550, 50), (63, 63, 63), -1) + cv.putText(visu_frame.matrix, f'Segment time: {int(video_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) + + # Draw focus area + cv.rectangle(visu_frame.matrix, (int(video_frame.width/6), 0), (int(visu_frame.width*(1-1/6)), int(visu_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: # Read data stream data_stream = tobii_data_stream.read() - # Store received gaze positions + # Store last received data + past_head_rotations.append(data_stream['Gyroscope']) past_gaze_positions.append(data_stream['GazePosition']) + # 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) - # 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: + gaze_position_pixel = (int(nearest_gaze_position.value[0] * visu_frame.width), int(nearest_gaze_position.value[1] * visu_frame.height)) - gaze_position_pixel = (int(nearest_gaze_position.value[0] * video_frame.width), int(nearest_gaze_position.value[1] * video_frame.height)) + # Draw gaze position + cv.circle(visu_frame.matrix, gaze_position_pixel, 2, (0, 255, 255), -1) - 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)) + # 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) - # 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) + # 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') - else: + gaze_position_pixel = (int(nearest_gaze_position.value[0] * visu_frame.width), int(nearest_gaze_position.value[1] * visu_frame.height)) - ValueError('Unable to evaluate gaze precision') + 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)) - # Wait for gaze position - except (AttributeError, ValueError): - continue + # Draw gaze position and accuracy + cv.circle(visu_frame.matrix, gaze_position_pixel, 2, (0, 255, 255), -1) + cv.circle(visu_frame.matrix, gaze_position_pixel, gaze_accuracy_pixel, (0, 255, 255), 1) - # 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) + # 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) - # 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) + # Track markers with pose estimation and draw them + aruco_tracker.track(video_frame.matrix) + aruco_tracker.draw(visu_frame.matrix) - # Project 3D scenes related to each aruco markers - if aruco_tracker.get_markers_number(): + if aruco_tracker.get_markers_number() == 0: + raise UserWarning('No marker detected') # Store aoi 2D video for further scene merging aoi2D_dict = {} + # Project 3D scenes related to each aruco markers for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()): - # Select 3D scene related to detected marker - aoi3D_scene = aoi3D_scene_selector(marker_id) - - if aoi3D_scene == None: - continue + # Process marker pose + try: - # 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 + # Select 3D scene related to detected marker + aoi3D_scene = aoi3D_scene_selector(marker_id) + + if aoi3D_scene == None: + raise UserWarning('No AOI 3D scene') - if distance_to_center > int(video_frame.width/3): - continue + # Transform scene into camera referential + aoi3D_camera = aoi3D_scene.transform(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i)) - # Transform scene into camera referential - aoi3D_camera = aoi3D_scene.transform(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i)) + # 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 - # 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 + aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(cone_vision_radius_cm, cone_vision_height_cm) - aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(cone_vision_radius_cm, cone_vision_height_cm) + # Keep only aoi inside vision cone field + aoi3D_scene = aoi3D_scene.copy(exclude=aoi3D_outside.keys()) - # Keep only aoi inside vision cone field - aoi3D_scene = aoi3D_scene.copy(exclude=aoi3D_outside.keys()) + # 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()) - # 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()) + # Store each 2D aoi for further scene merging + for name, aoi in aoi2D_video_scene.items(): - # Store each 2D aoi for further scene merging - for name, aoi in aoi2D_video_scene.items(): + if name not in aoi2D_dict.keys(): + aoi2D_dict[name] = [] - if name not in aoi2D_dict.keys(): - aoi2D_dict[name] = [] + aoi2D_dict[name].append(aoi.clockwise()) - aoi2D_dict[name].append(aoi.clockwise()) + # 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) # 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(video_frame.matrix, video_gaze_pixel, gaze_accuracy_pixel, exclude=['Visualisation_Plan']) + aoi2D_merged_scene.draw(visu_frame.matrix, video_gaze_pixel, gaze_accuracy_pixel, exclude=['Visualisation_Plan']) # Send look at aoi pointer for name, aoi in aoi2D_merged_scene.items(): @@ -207,11 +247,19 @@ def main(): else: IvySendMsg(f'looking {name}') + # 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 + # Close window using 'Esc' key if cv.waitKey(1) == 27: break - cv.imshow('Live Scene', video_frame.matrix) + cv.imshow('Live Scene', visu_frame.matrix) # Exit on 'ctrl+C' interruption except KeyboardInterrupt: |