From 214a057db794ced4fed7722912a571297d2a64de Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 23 Nov 2022 09:35:40 +0100 Subject: Adding a new utils script to show how to use ArUcoPlan class. --- src/argaze/utils/tobii_stream_arcube_display.py | 359 -------------------- .../utils/tobii_stream_aruco_plan_display.py | 360 +++++++++++++++++++++ 2 files changed, 360 insertions(+), 359 deletions(-) delete mode 100644 src/argaze/utils/tobii_stream_arcube_display.py create mode 100644 src/argaze/utils/tobii_stream_aruco_plan_display.py diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py deleted file mode 100644 index 9f4a196..0000000 --- a/src/argaze/utils/tobii_stream_arcube_display.py +++ /dev/null @@ -1,359 +0,0 @@ -#!/usr/bin/env python - -import argparse -import os, json -import math -import threading - -from argaze import DataStructures -from argaze import GazeFeatures -from argaze.TobiiGlassesPro2 import * -from argaze.ArUcoMarkers import * -from argaze.AreaOfInterest import * -from argaze.utils import MiscFeatures - -import cv2 as cv -import numpy - -def make_rotation_matrix(x, y, z): - - # Create rotation matrix around x axis - c = numpy.cos(numpy.deg2rad(x)) - s = numpy.sin(numpy.deg2rad(x)) - Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) - - # Create rotation matrix around y axis - c = numpy.cos(numpy.deg2rad(y)) - s = numpy.sin(numpy.deg2rad(y)) - Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) - - # Create rotation matrix around z axis - c = numpy.cos(numpy.deg2rad(z)) - s = numpy.sin(numpy.deg2rad(z)) - Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) - - # Return intrinsic rotation matrix - return Rx.dot(Ry.dot(Rz)) - -def main(): - """ - Track ArUcoCube into Tobii Glasses Pro 2 camera video stream. - """ - - # Manage arguments - parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0]) - parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip') - 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('-i', '--imu_calibration', metavar='IMU_CALIB', type=str, default=None, help='json imu calibration filepath') - parser.add_argument('-ac', '--aruco_cube', metavar='ARUCO_CUBE', type=str, help='json aruco cube description filepath') - parser.add_argument('-s', '--aoi_scene', metavar='AOI_SCENE', type=str, help='obj aoi 3D scene description filepath') - parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction) - args = parser.parse_args() - - # Create tobii controller (with auto discovery network process if no ip argument is provided) - print('\nLooking for a Tobii Glasses Pro 2 device ...') - - try: - - tobii_controller = TobiiController.TobiiController(args.tobii_ip) - print(f'Tobii Glasses Pro 2 device found at {tobii_controller.address} address.') - - except ConnectionError as e: - - print(e) - exit() - - # Setup camera at 25 fps to work on Full HD video stream - tobii_controller.set_scene_camera_freq_25() - - # Print current confirugration - print(f'Tobii Glasses Pro 2 configuration:') - for key, value in tobii_controller.get_configuration().items(): - print(f'\t{key}: {value}') - - # Enable tobii data stream - tobii_data_stream = tobii_controller.enable_data_stream() - - # Enable tobii video stream - tobii_video_stream = tobii_controller.enable_video_stream() - - # Load aruco cube description - aruco_cube = ArUcoCube.ArUcoCube(args.aruco_cube) - aruco_cube.print_cache() - - # Load AOI 3D scene centered onto aruco cube - aoi3D_scene = AOI3DScene.AOI3DScene() - aoi3D_scene.load(args.aoi_scene) - - print(f'\nAOI in {os.path.basename(args.aoi_scene)} scene related to ArCube:') - for aoi in aoi3D_scene.keys(): - print(f'\t{aoi}') - - # Create aruco camera - aruco_camera = ArUcoCamera.ArUcoCamera() - - # 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(aruco_cube.dictionary, aruco_cube.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'\nArUcoTracker configuration for markers detection:') - aruco_tracker.print_configuration() - - # Create tobii imu handler to track head pose changes when arcuco cube pose can't be estimated - # So, the resulting head pose is relative to last pose estimation - tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit() - - # Load optional imu calibration file - if args.imu_calibration != None: - - tobii_imu.load_calibration_file(args.imu_calibration) - - # Init tobii imu lock - tobii_imu_lock = threading.Lock() - - # TEST : DIFF ACC - last_accl = numpy.zeros(3) - - # Init data timestamped in millisecond - data_ts_ms = 0 - - # Assess temporal performance - loop_chrono = MiscFeatures.TimeProbe() - loop_ps = 0 - - def data_stream_callback(data_ts, data_object, data_object_type): - - nonlocal tobii_imu - nonlocal tobii_imu_lock - nonlocal data_ts_ms - - #TEST - nonlocal last_accl - - data_ts_ms = data_ts / 1e3 - - # Don't update imu when it is used - if tobii_imu_lock.locked(): - return - - # Lock tobii imu updates - tobii_imu_lock.acquire() - - match data_object_type: - - case 'Gyroscope': - - data_object = tobii_imu.apply_gyroscope_offset(data_object) - - tobii_imu.update_rotation(data_ts, data_object) - - - case 'Accelerometer': - pass - ''' - print('raw accelerometer(m/s2)=', data_object.value) - - # TEST : - diff_accl = last_accl - numpy.array(data_object.value) - last_accl = numpy.array(data_object.value) - print('\tdiff(cm/s2)=', 100 * numpy.linalg.norm(diff_accl)) - - # TEST : ignore acceleration double - if numpy.linalg.norm(diff_accl) > 0.: - - data_object = tobii_imu.apply_accelerometer_coefficients(data_object) - - print('corrected accelerometer(m/s2)=', data_object.value) - - print('current plumb=', tobii_imu.get_plumb()) - - data_object = tobii_imu.apply_plumb(data_object) - - print('corrected accelerometer - gravity(m/s2)=', data_object.value) - print('\tnorm(cm/s2)=', 100 * numpy.linalg.norm(data_object.value)) - - tobii_imu.update_translation(data_ts, data_object) - ''' - # Unlock tobii imu updates - tobii_imu_lock.release() - - tobii_data_stream.reading_callback = data_stream_callback - - # Start streaming - tobii_controller.start_streaming() - - # Live video stream capture loop - try: - - # Assess loop performance - loop_chrono = MiscFeatures.TimeProbe() - fps = 0 - - # Track aruco cube pose - aruco_cube_tvec = numpy.zeros(3) - aruco_cube_rvec = numpy.zeros(3) - aruco_cube_success = False - aruco_cube_validity = False - aruco_cube_ts_ms = 0 - - while tobii_video_stream.is_alive(): - - # Read video stream - video_ts, video_frame = tobii_video_stream.read() - video_ts_ms = video_ts / 1e3 - - # Copy video frame to edit visualisation on it without disrupting aruco tracking - visu_frame = video_frame.copy() - - # Process video and data frame - try: - - # Track markers with pose estimation - aruco_tracker.track(video_frame.matrix) - - # Estimate cube pose from tracked markers - tvec, rvec, success, validity = aruco_cube.estimate_pose(aruco_tracker.tracked_markers) - - # Cube pose estimation succeed and is validated by 2 faces at least - if success and validity >= 2: - - # Lock tobii imu updates - tobii_imu_lock.acquire() - - # Reset head rotation, translation and translation speed (cm/s) - # Note : head translation speed is also estimated thanks to accelerometer sensor (see upward) - tobii_imu.reset_rotation() - #tobii_imu.reset_translation(translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms)) - - # Create a rotation matrix to transform cube rotation from camera referential to imu referential - F = make_rotation_matrix(*TobiiInertialMeasureUnit.CAMERA_TO_IMU_ROTATION_VECTOR) - R, _ = cv.Rodrigues(rvec) - rvec_flipped, _ = cv.Rodrigues(F.dot(R)) - - # Update head plumb orientation with flipped cube orientation - tobii_imu.rotate_plumb(rvec_flipped) - - # Unlock tobii imu updates - tobii_imu_lock.release() - - # Store cube pose - aruco_cube_tvec = tvec - aruco_cube_rvec = rvec - aruco_cube_success = success - aruco_cube_validity = validity - aruco_cube_ts_ms = video_ts_ms - - # Cube pose estimation fails - elif aruco_cube_success: - - # Use tobii glasses inertial sensors to estimate cube pose from last estimated pose - - # Translate cube into imu referential - imu_tvec = aruco_cube_tvec + numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR) - - # Translate cube according head translation - imu_tvec = imu_tvec + tobii_imu.translation - - # Rotate cube around imu origin according head rotation - imu_rvec = tobii_imu.rotation * numpy.array([-1., -1., 1.]) - imu_R = make_rotation_matrix(*imu_rvec) - new_imu_tvec = imu_tvec.dot(imu_R) - - # Translate back cube into camera referential - new_tvec = new_imu_tvec - numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR) - - # Rotate cube orientation (supposing cube top is up in ) - imu_rvec = tobii_imu.rotation * numpy.array([1., -1., 1.]) - imu_R = make_rotation_matrix(*imu_rvec) - - C, _ = cv.Rodrigues(aruco_cube_rvec) - C = C.dot(imu_R) - new_rvec, _ = cv.Rodrigues(C) - #new_rvec = aruco_cube_rvec - - # Set cube pose estimation - aruco_cube.translation = new_tvec - aruco_cube.rotation = new_rvec - - else: - - raise UserWarning('Cube pose estimation fails.') - - # Project AOI 3 scene onto camera frame - - # 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_scene = aoi3D_scene.project(aruco_cube.translation, aruco_cube.rotation, aruco_camera.K) - - # Draw projected scene - #aoi2D_scene.draw(visu_frame.matrix) - - # Draw markers pose estimation - #aruco_tracker.draw_tracked_markers(visu_frame.matrix) - - # Draw cube pose estimation (without camera distorsion) - aruco_cube.draw(visu_frame.matrix, aruco_camera.K, aruco_camera.D, draw_faces=False) - - # Warn about cube pose validity - if not aruco_cube_validity: - - raise UserWarning('Cube pose estimation is not validated.') - - # Write warning - except UserWarning as w: - - cv.rectangle(visu_frame.matrix, (0, 100), (600, 150), (127, 127, 127), -1) - cv.putText(visu_frame.matrix, str(w), (20, 140), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) - - # Assess loop performance - lap_time, lap_counter, elapsed_time = loop_chrono.lap() - - # Update fps each 10 loops - if lap_counter >= 10: - - loop_ps = 1e3 * lap_counter / elapsed_time - loop_chrono.restart() - - # 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) - - # Write stream timing - cv.rectangle(visu_frame.matrix, (0, 0), (1100, 50), (63, 63, 63), -1) - cv.putText(visu_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(visu_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) - cv.putText(visu_frame.matrix, f'Fps: {int(loop_ps)}', (950, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) - - cv.imshow(f'Stream ArUcoCube', visu_frame.matrix) - - # Close window using 'Esc' key - if cv.waitKey(1) == 27: - break - - # Exit on 'ctrl+C' interruption - except KeyboardInterrupt: - pass - - # Stop frame display - cv.destroyAllWindows() - - # Stop streaming - tobii_controller.stop_streaming() - -if __name__ == '__main__': - - main() \ No newline at end of file diff --git a/src/argaze/utils/tobii_stream_aruco_plan_display.py b/src/argaze/utils/tobii_stream_aruco_plan_display.py new file mode 100644 index 0000000..16fc8ef --- /dev/null +++ b/src/argaze/utils/tobii_stream_aruco_plan_display.py @@ -0,0 +1,360 @@ +#!/usr/bin/env python + +import argparse +import os, json +import math +import threading + +from argaze import DataStructures +from argaze import GazeFeatures +from argaze.TobiiGlassesPro2 import * +from argaze.ArUcoMarkers import * +from argaze.AreaOfInterest import * +from argaze.utils import MiscFeatures + +import cv2 as cv +import numpy + +def make_rotation_matrix(x, y, z): + + # Create rotation matrix around x axis + c = numpy.cos(numpy.deg2rad(x)) + s = numpy.sin(numpy.deg2rad(x)) + Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) + + # Create rotation matrix around y axis + c = numpy.cos(numpy.deg2rad(y)) + s = numpy.sin(numpy.deg2rad(y)) + Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) + + # Create rotation matrix around z axis + c = numpy.cos(numpy.deg2rad(z)) + s = numpy.sin(numpy.deg2rad(z)) + Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + + # Return intrinsic rotation matrix + return Rx.dot(Ry.dot(Rz)) + +def main(): + """ + Track ArUcoPlan into Tobii Glasses Pro 2 camera video stream. + """ + + # Manage arguments + parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0]) + parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip') + 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('-i', '--imu_calibration', metavar='IMU_CALIB', type=str, default=None, help='json imu calibration filepath') + parser.add_argument('-ac', '--aruco_plan', metavar='ARUCO_PLAN', type=str, help='json aruco plan description filepath') + parser.add_argument('-s', '--aoi_scene', metavar='AOI_SCENE', type=str, help='obj aoi 3D scene description filepath') + parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction) + args = parser.parse_args() + + # Create tobii controller (with auto discovery network process if no ip argument is provided) + print('\nLooking for a Tobii Glasses Pro 2 device ...') + + try: + + tobii_controller = TobiiController.TobiiController(args.tobii_ip) + print(f'Tobii Glasses Pro 2 device found at {tobii_controller.address} address.') + + except ConnectionError as e: + + print(e) + exit() + + # Setup camera at 25 fps to work on Full HD video stream + tobii_controller.set_scene_camera_freq_25() + + # Print current confirugration + print(f'Tobii Glasses Pro 2 configuration:') + for key, value in tobii_controller.get_configuration().items(): + print(f'\t{key}: {value}') + + # Enable tobii data stream + tobii_data_stream = tobii_controller.enable_data_stream() + + # Enable tobii video stream + tobii_video_stream = tobii_controller.enable_video_stream() + + # Load aruco plan description + aruco_plan = ArUcoPlan.ArUcoPlan(args.aruco_plan) + aruco_plan.print_cache() + + # Load AOI 3D scene centered onto aruco plan + aoi3D_scene = AOI3DScene.AOI3DScene() + aoi3D_scene.load(args.aoi_scene) + + print(f'\nAOI in {os.path.basename(args.aoi_scene)} scene related to ArPlan:') + for aoi in aoi3D_scene.keys(): + print(f'\t{aoi}') + + # Create aruco camera + aruco_camera = ArUcoCamera.ArUcoCamera() + + # 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(aruco_plan.dictionary, aruco_plan.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'\nArUcoTracker configuration for markers detection:') + aruco_tracker.print_configuration() + + # Create tobii imu handler to track head pose changes when arcuco plan pose can't be estimated + # So, the resulting head pose is relative to last pose estimation + tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit() + + # Load optional imu calibration file + if args.imu_calibration != None: + + tobii_imu.load_calibration_file(args.imu_calibration) + + # Init tobii imu lock + tobii_imu_lock = threading.Lock() + + # TEST : DIFF ACC + last_accl = numpy.zeros(3) + + # Init data timestamped in millisecond + data_ts_ms = 0 + + # Assess temporal performance + loop_chrono = MiscFeatures.TimeProbe() + loop_ps = 0 + + def data_stream_callback(data_ts, data_object, data_object_type): + + nonlocal tobii_imu + nonlocal tobii_imu_lock + nonlocal data_ts_ms + + #TEST + nonlocal last_accl + + data_ts_ms = data_ts / 1e3 + + # Don't update imu when it is used + if tobii_imu_lock.locked(): + return + + # Lock tobii imu updates + tobii_imu_lock.acquire() + + match data_object_type: + + case 'Gyroscope': + + data_object = tobii_imu.apply_gyroscope_offset(data_object) + + tobii_imu.update_rotation(data_ts, data_object) + + + case 'Accelerometer': + pass + ''' + print('raw accelerometer(m/s2)=', data_object.value) + + # TEST : + diff_accl = last_accl - numpy.array(data_object.value) + last_accl = numpy.array(data_object.value) + print('\tdiff(cm/s2)=', 100 * numpy.linalg.norm(diff_accl)) + + # TEST : ignore acceleration double + if numpy.linalg.norm(diff_accl) > 0.: + + data_object = tobii_imu.apply_accelerometer_coefficients(data_object) + + print('corrected accelerometer(m/s2)=', data_object.value) + + print('current plumb=', tobii_imu.get_plumb()) + + data_object = tobii_imu.apply_plumb(data_object) + + print('corrected accelerometer - gravity(m/s2)=', data_object.value) + print('\tnorm(cm/s2)=', 100 * numpy.linalg.norm(data_object.value)) + + tobii_imu.update_translation(data_ts, data_object) + ''' + # Unlock tobii imu updates + tobii_imu_lock.release() + + tobii_data_stream.reading_callback = data_stream_callback + + # Start streaming + tobii_controller.start_streaming() + + # Live video stream capture loop + try: + + # Assess loop performance + loop_chrono = MiscFeatures.TimeProbe() + fps = 0 + + # Track aruco plan pose + aruco_plan_tvec = numpy.zeros(3) + aruco_plan_rvec = numpy.zeros(3) + aruco_plan_success = False + aruco_plan_validity = False + aruco_plan_ts_ms = 0 + + while tobii_video_stream.is_alive(): + + # Read video stream + video_ts, video_frame = tobii_video_stream.read() + video_ts_ms = video_ts / 1e3 + + # Copy video frame to edit visualisation on it without disrupting aruco tracking + visu_frame = video_frame.copy() + + # Process video and data frame + try: + + # Track markers with pose estimation + aruco_tracker.track(video_frame.matrix) + #aruco_tracker.draw_tracked_markers(visu_frame.matrix) + + # Estimate plan pose from tracked markers + tvec, rvec, success, validity = aruco_plan.estimate_pose(aruco_tracker.tracked_markers) + + # Plan pose estimation succeed and is validated by 1 faces at least + if success and validity >= 1: + + # Lock tobii imu updates + tobii_imu_lock.acquire() + + # Reset head rotation, translation and translation speed (cm/s) + # Note : head translation speed is also estimated thanks to accelerometer sensor (see upward) + tobii_imu.reset_rotation() + #tobii_imu.reset_translation(translation_speed = (tvec - aruco_plan_tvec) / (video_ts_ms - aruco_plan_ts_ms)) + + # Create a rotation matrix to transform plan rotation from camera referential to imu referential + F = make_rotation_matrix(*TobiiInertialMeasureUnit.CAMERA_TO_IMU_ROTATION_VECTOR) + R, _ = cv.Rodrigues(rvec) + rvec_flipped, _ = cv.Rodrigues(F.dot(R)) + + # Update head plumb orientation with flipped plan orientation + tobii_imu.rotate_plumb(rvec_flipped) + + # Unlock tobii imu updates + tobii_imu_lock.release() + + # Store plan pose + aruco_plan_tvec = tvec + aruco_plan_rvec = rvec + aruco_plan_success = success + aruco_plan_validity = validity + aruco_plan_ts_ms = video_ts_ms + + # Plan pose estimation fails + elif aruco_plan_success: + + # Use tobii glasses inertial sensors to estimate plan pose from last estimated pose + + # Translate plan into imu referential + imu_tvec = aruco_plan_tvec + numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR) + + # Translate plan according head translation + imu_tvec = imu_tvec + tobii_imu.translation + + # Rotate plan around imu origin according head rotation + imu_rvec = tobii_imu.rotation * numpy.array([-1., -1., 1.]) + imu_R = make_rotation_matrix(*imu_rvec) + new_imu_tvec = imu_tvec.dot(imu_R) + + # Translate back plan into camera referential + new_tvec = new_imu_tvec - numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR) + + # Rotate plan orientation (supposing plan top is up in ) + imu_rvec = tobii_imu.rotation * numpy.array([1., -1., 1.]) + imu_R = make_rotation_matrix(*imu_rvec) + + C, _ = cv.Rodrigues(aruco_plan_rvec) + C = C.dot(imu_R) + new_rvec, _ = cv.Rodrigues(C) + #new_rvec = aruco_plan_rvec + + # Set plan pose estimation + aruco_plan.translation = new_tvec + aruco_plan.rotation = new_rvec + + else: + + raise UserWarning('Plan pose estimation fails.') + + # Project AOI 3 scene onto camera frame + + # 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_scene = aoi3D_scene.project(aruco_plan.translation, aruco_plan.rotation, aruco_camera.K) + + # Draw projected scene + #aoi2D_scene.draw(visu_frame.matrix) + + # Draw markers pose estimation + #aruco_tracker.draw_tracked_markers(visu_frame.matrix) + + # Draw plan pose estimation (without camera distorsion) + aruco_plan.draw(visu_frame.matrix, aruco_camera.K, ArUcoCamera.D0, draw_places=True) + + # Warn about plan pose validity + if not aruco_plan_validity: + + raise UserWarning('Plan pose estimation is not validated.') + + # Write warning + except UserWarning as w: + + cv.rectangle(visu_frame.matrix, (0, 100), (600, 150), (127, 127, 127), -1) + cv.putText(visu_frame.matrix, str(w), (20, 140), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) + + # Assess loop performance + lap_time, lap_counter, elapsed_time = loop_chrono.lap() + + # Update fps each 10 loops + if lap_counter >= 10: + + loop_ps = 1e3 * lap_counter / elapsed_time + loop_chrono.restart() + + # 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) + + # Write stream timing + cv.rectangle(visu_frame.matrix, (0, 0), (1100, 50), (63, 63, 63), -1) + cv.putText(visu_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(visu_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) + cv.putText(visu_frame.matrix, f'Fps: {int(loop_ps)}', (950, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) + + cv.imshow(f'Stream ArUcoPlan', visu_frame.matrix) + + # Close window using 'Esc' key + if cv.waitKey(1) == 27: + break + + # Exit on 'ctrl+C' interruption + except KeyboardInterrupt: + pass + + # Stop frame display + cv.destroyAllWindows() + + # Stop streaming + tobii_controller.stop_streaming() + +if __name__ == '__main__': + + main() \ No newline at end of file -- cgit v1.1