diff options
| -rw-r--r-- | src/argaze/TobiiGlassesPro2/TobiiData.py | 4 | ||||
| -rw-r--r-- | src/argaze/utils/tobii_stream_aruco_aoi_display.py | 60 | ||||
| -rw-r--r-- | src/argaze/utils/tobii_stream_aruco_aoi_ivy_controller.py | 13 | ||||
| -rw-r--r-- | src/argaze/utils/tobii_stream_display.py | 100 |
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: |