From 50599050d17d96607c232e6c57c2256aef01d16c Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Mon, 9 May 2022 15:38:56 +0200 Subject: Adding a util script to show controller-application communication through Ivy bus --- .../utils/live_tobii_aruco_aoi_ivy_application.py | 122 +++++++++++++++++ .../utils/live_tobii_aruco_aoi_ivy_controller.py | 152 +++++++++++++++++++++ src/argaze/utils/live_tobii_aruco_aois.py | 140 ------------------- 3 files changed, 274 insertions(+), 140 deletions(-) create mode 100644 src/argaze/utils/live_tobii_aruco_aoi_ivy_application.py create mode 100644 src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py delete mode 100644 src/argaze/utils/live_tobii_aruco_aois.py diff --git a/src/argaze/utils/live_tobii_aruco_aoi_ivy_application.py b/src/argaze/utils/live_tobii_aruco_aoi_ivy_application.py new file mode 100644 index 0000000..5737bbc --- /dev/null +++ b/src/argaze/utils/live_tobii_aruco_aoi_ivy_application.py @@ -0,0 +1,122 @@ + #!/usr/bin/env python + +import argparse +import os + +from argaze import DataStructures, GazeFeatures +from argaze.ArUcoMarkers import ArUcoMarkersDictionary +from argaze.AreaOfInterest import * + +import cv2 as cv +import numpy + +from ivy.std_api import * + +def main(): + """ + Define screen as an ArUco AOI scene scene and bind to Ivy default bus to receive live look at pointer data. + """ + + # Manage arguments + parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0]) + parser.add_argument('-y', '--ivy_bus', metavar='IVY_BUS', type=str, default='0.0.0.0:2010', help='Ivy bus ip and port') + parser.add_argument('-a', '--aoi_scene', metavar='AOI_SCENE', type=str, default='aoi3D_scene.obj', help='obj aoi scene 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('-i', '--marker_id', metavar='MARKER_ID', type=int, default=0, help='marker id to display') + args = parser.parse_args() + + # Enable Ivy bus + IvyInit(os.path.basename(__file__)) + IvyStart(args.ivy_bus) + + def on_looking_message(*args): + + look_at = numpy.fromstring(args[2].replace('[','').replace(']',''), dtype=float, count=2, sep=', ') + + visu_gaze_pixel = aoi2D_visu_scene[args[1]].looked_pixel(look_at) + + cv.circle(visu_frame, visu_gaze_pixel, 4, (0, 0, 255), -1) + + IvyBindMsg(on_looking_message, 'looking (.*) at (.*)') + + # Create AOIs 3D scene + aoi3D_scene = AOI3DScene.AOI3DScene() + aoi3D_scene.load(args.aoi_scene) + print(f'AOIs names: {aoi3D_scene.keys()}') + + # Create a visual scan visualisation frame + visu_width = 1920 + visu_height = 1080 + visu_ratio = visu_height + visu_frame = numpy.full((visu_height, visu_width, 3), 255, dtype=numpy.uint8) + + cv.imshow('Scene', visu_frame) + + # Project 3D scene on the reference frame + # TODO : center projection on a reference AOI + ref_aoi = 'Scene_Plan' + + # TODO: pass the reference AOI in argument + aoi3D_scene.rotation = numpy.asarray([[-numpy.pi, 0.0, 0.0]]) + aoi3D_scene.translation = numpy.asarray([[19.0, 8.0, 25.0]]) + + # Edit a projection matrix for the reference frame + K0 = numpy.asarray([[visu_ratio, 0.0, visu_width/2], [0.0, visu_ratio, visu_height/2], [0.0, 0.0, 1.0]]) + + aoi2D_visu_scene = aoi3D_scene.project(K0) + + # Create aruco markers dictionary + aruco_markers_dict = ArUcoMarkersDictionary.ArUcoMarkersDictionary(args.dictionary) + + # Create aruco marker + marker_box = aoi2D_visu_scene['Marker_Plan'].bounding_box().astype(int) + marker_size = marker_box[2] - marker_box[0] + marker = aruco_markers_dict.create_marker(args.marker_id, int(marker_size[0])) + print(f'Creating Aruco marker {args.marker_id} from the {args.dictionary} dictionary') + + def draw_scene(): + + # Clear frame + visu_frame[:] = 255 + + # Display AOI 2D scene + for name, aoi in aoi2D_visu_scene.items(): + aoi.draw(visu_frame, (0, 0, 0)) + + # Display aruco marker + visu_frame[marker_box[0][1]:marker_box[2][1], marker_box[0][0]:marker_box[2][0], :] = marker + + # On mouse over : redraw scene and draw target + def on_mouse_event(event, x, y, flags, param): + + draw_scene() + + # Draw target + cv.circle(visu_frame, (x, y), 40, (0, 255, 255), -1) + + cv.setMouseCallback('Scene', on_mouse_event) + + # Screen display loop + try: + + draw_scene() + + while True: + + # Close window using 'Esc' key + if cv.waitKey(1) == 27: + break + + cv.imshow('Scene', visu_frame) + + # Exit on 'ctrl+C' interruption + except KeyboardInterrupt: + pass + + # Stop frame display + cv.destroyAllWindows() + +if __name__ == '__main__': + + main() \ No newline at end of file diff --git a/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py b/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py new file mode 100644 index 0000000..3129fa4 --- /dev/null +++ b/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py @@ -0,0 +1,152 @@ + #!/usr/bin/env python + +import argparse +import os + +from argaze import DataStructures, GazeFeatures +from argaze.TobiiGlassesPro2 import * +from argaze.ArUcoMarkers import ArUcoTracker, ArUcoCamera +from argaze.AreaOfInterest import * +from argaze.TobiiGlassesPro2 import * + +import cv2 as cv +import numpy + +from ivy.std_api import * + +def main(): + """ + Track any ArUco marker into Tobii Glasses Pro 2 camera video stream. + From a loaded AOI scene .obj file, position the scene virtually relatively to any detected ArUco markers and project the scene into camera frame. + Then, detect if Tobii gaze point is inside any AOI and send the look at pointer over Ivy default bus. + """ + + # 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('-y', '--ivy_bus', metavar='IVY_BUS', type=str, default='0.0.0.0:2010', help='Ivy bus ip and port') + parser.add_argument('-a', '--aoi_scene', metavar='AOI_SCENE', type=str, default='aoi3D_scene.obj', help='obj aoi scene 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('-i', '--markers_id', metavar='MARKERS_ID', nargs='*', type=int, default=[], help='markers id to track') + args = parser.parse_args() + + empty_marker_set = len(args.markers_id) == 0 + if empty_marker_set: + print(f'Track any Aruco markers from the {args.dictionary} dictionary') + else: + print(f'Track Aruco markers {args.markers_id} from the {args.dictionary} dictionary') + + # Enable Ivy bus + IvyInit(os.path.basename(__file__)) + IvyStart(args.ivy_bus) + + # 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() + + # Enable tobii video stream + tobii_video_stream = tobii_controller.enable_video_stream() + + # create aruco camera + aruco_camera = ArUcoCamera.ArUcoCamera() + aruco_camera.load_calibration_file(args.camera_calibration) + + # Create aruco tracker + aruco_tracker = ArUcoTracker.ArUcoTracker(args.dictionary, args.marker_size, aruco_camera) + + # Create AOIs 3D scene + aoi3D_scene = AOI3DScene.AOI3DScene() + aoi3D_scene.load(args.aoi_scene) + print(f'AOIs names: {aoi3D_scene.keys()}') + + # Start streaming + tobii_controller.start_streaming() + + # Live video stream capture loop + try: + + past_gaze_positions = DataStructures.TimeStampedBuffer() + + while tobii_video_stream.is_alive(): + + video_ts, video_frame = tobii_video_stream.read() + + try: + + # Read data stream + data_stream = tobii_data_stream.read() + + # Store received gaze positions + past_gaze_positions.append(data_stream.gidx_l_gp) + + # 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) + + # Draw gaze position + video_gaze_pixel = (int(earliest_gaze_position.gp[0] * video_frame.width), int(earliest_gaze_position.gp[1] * video_frame.height)) + cv.circle(video_frame.matrix, video_gaze_pixel, 4, (0, 255, 255), -1) + + # Wait for gaze position + except (AttributeError, ValueError): + continue + + # Track markers with pose estimation and draw them + aruco_tracker.track(video_frame.matrix) + aruco_tracker.draw(video_frame.matrix) + + # Project 3D scenes related to each aruco markers + if aruco_tracker.get_markers_number(): + + for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()): + + # TODO : Select different 3D scenes depending on aruco id + + in_marker_set = marker_id in list(args.markers_id) + + if not empty_marker_set and not in_marker_set: + continue + + aoi3D_scene.rotation = aruco_tracker.get_marker_rotation(i) + aoi3D_scene.translation = aruco_tracker.get_marker_translation(i) + + # Edit Zero distorsion matrix + D0 = numpy.asarray([0.0, 0.0, 0.0, 0.0, 0.0]) + + # 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_camera.get_K(), D0) + + # Draw 2D scene + aoi2D_scene.draw(video_frame.matrix, video_gaze_pixel) + + # Send look at aoi pointer + for name, aoi in aoi2D_scene.items(): + if aoi.looked(video_gaze_pixel): + IvySendMsg(f'looking {name} at {aoi.look_at(video_gaze_pixel)}') + + # Close window using 'Esc' key + if cv.waitKey(1) == 27: + break + + cv.imshow('Live Scene', video_frame.matrix) + + # 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/live_tobii_aruco_aois.py b/src/argaze/utils/live_tobii_aruco_aois.py deleted file mode 100644 index bf91eec..0000000 --- a/src/argaze/utils/live_tobii_aruco_aois.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python - -import argparse -import os - -from argaze import DataStructures, GazeFeatures -from argaze.TobiiGlassesPro2 import * -from argaze.ArUcoMarkers import ArUcoTracker, ArUcoCamera -from argaze.AreaOfInterest import * -from argaze.TobiiGlassesPro2 import * - -import cv2 as cv -import numpy - -def main(): - """ - Track any ArUco marker into Tobii Glasses Pro 2 camera video stream. - From a loaded AOI scene .obj file, position the scene virtually relatively to any detected ArUco markers and project the scene into camera frame. - Then, detect if Tobii gaze point is inside any AOI. - """ - - # 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('-a', '--aoi_scene', metavar='AOI_SCENE', type=str, default='aoi3D_scene.obj', help='obj aoi scene 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('-i', '--markers_id', metavar='MARKERS_ID', nargs='*', type=int, default=[], help='markers id to track') - args = parser.parse_args() - - empty_marker_set = len(args.markers_id) == 0 - if empty_marker_set: - print(f'Track any Aruco markers from the {args.dictionary} dictionary') - else: - print(f'Track Aruco markers {args.markers_id} from the {args.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() - - # Enable tobii video stream - tobii_video_stream = tobii_controller.enable_video_stream() - - # create aruco camera - aruco_camera = ArUcoCamera.ArUcoCamera() - aruco_camera.load_calibration_file(args.camera_calibration) - - # Create aruco tracker - aruco_tracker = ArUcoTracker.ArUcoTracker(args.dictionary, args.marker_size, aruco_camera) - - # Create AOIs 3D scene - aoi3D_scene = AOI3DScene.AOI3DScene() - aoi3D_scene.load(args.aoi_scene) - print(f'AOIs names: {aoi3D_scene.keys()}') - - # Start streaming - tobii_controller.start_streaming() - - # Live video stream capture loop - try: - - past_gaze_positions = DataStructures.TimeStampedBuffer() - - while tobii_video_stream.is_alive(): - - video_ts, video_frame = tobii_video_stream.read() - - try: - - # Read data stream - data_stream = tobii_data_stream.read() - - # Store received gaze positions - past_gaze_positions.append(data_stream.gidx_l_gp) - - # 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) - - # Draw gaze position - gaze_position = (int(earliest_gaze_position.gp[0] * video_frame.width), int(earliest_gaze_position.gp[1] * video_frame.height)) - cv.circle(video_frame.matrix, gaze_position, 4, (0, 255, 255), -1) - - # Wait for gaze position - except (AttributeError, ValueError): - continue - - # Track markers with pose estimation and draw them - aruco_tracker.track(video_frame.matrix) - aruco_tracker.draw(video_frame.matrix) - - # Project 3D scenes related to each aruco markers - if aruco_tracker.get_markers_number(): - - for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()): - - # TODO : Select different 3D scenes depending on aruco id - - in_marker_set = marker_id in list(args.markers_id) - - if not empty_marker_set and not in_marker_set: - continue - - aoi3D_scene.rotation = aruco_tracker.get_marker_rotation(i) - aoi3D_scene.translation = aruco_tracker.get_marker_translation(i) - - # Edit Zero distorsion matrix - D0 = numpy.asarray([0.0, 0.0, 0.0, 0.0, 0.0]) - - # 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_camera.get_K(), D0) - - # Draw 2D scene - aoi2D_scene.draw(video_frame.matrix, gaze_position) - - # Close window using 'Esc' key - if cv.waitKey(1) == 27: - break - - cv.imshow('Live Scene', video_frame.matrix) - - # 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