From 72d6409892d5b9df05f60351bb06072b7f9e1951 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Mon, 24 Apr 2023 12:40:59 +0200 Subject: Adding PupilFeatures file and PupilAnalysis folder. --- src/argaze.test/PupilAnalysis/WorkloadIndex.py | 46 ++++ src/argaze.test/PupilAnalysis/__init__.py | 0 src/argaze.test/PupilFeatures.py | 160 ++++++++++++ src/argaze/PupilAnalysis/README.md | 5 + src/argaze/PupilAnalysis/WorkloadIndex.py | 51 ++++ src/argaze/PupilAnalysis/__init__.py | 5 + src/argaze/PupilFeatures.py | 99 +++++++ src/argaze/__init__.py | 2 +- src/argaze/utils/environment_edit.py | 343 +++++++++++++++++++++++++ 9 files changed, 710 insertions(+), 1 deletion(-) create mode 100644 src/argaze.test/PupilAnalysis/WorkloadIndex.py create mode 100644 src/argaze.test/PupilAnalysis/__init__.py create mode 100644 src/argaze.test/PupilFeatures.py create mode 100644 src/argaze/PupilAnalysis/README.md create mode 100644 src/argaze/PupilAnalysis/WorkloadIndex.py create mode 100644 src/argaze/PupilAnalysis/__init__.py create mode 100644 src/argaze/PupilFeatures.py create mode 100644 src/argaze/utils/environment_edit.py diff --git a/src/argaze.test/PupilAnalysis/WorkloadIndex.py b/src/argaze.test/PupilAnalysis/WorkloadIndex.py new file mode 100644 index 0000000..98a1e45 --- /dev/null +++ b/src/argaze.test/PupilAnalysis/WorkloadIndex.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python + +import unittest +import math + +from argaze import PupilFeatures +from argaze.PupilAnalysis import WorkloadIndex + +class TestWorkloadIndexClass(unittest.TestCase): + """Test WorkloadIndex class.""" + + def test_analysis(self): + """Test WorkloadIndex analysis.""" + + ts_pupil_diameters = { + 0: PupilFeatures.PupilDiameter(1.), + 1: PupilFeatures.PupilDiameter(1.1), + 2: PupilFeatures.PupilDiameter(1.2), + 3: PupilFeatures.PupilDiameter(1.3), + 4: PupilFeatures.PupilDiameter(1.2), + 5: PupilFeatures.PupilDiameter(1.1), + 6: PupilFeatures.PupilDiameter(1.), + 7: PupilFeatures.PupilDiameter(0.9), + 8: PupilFeatures.PupilDiameter(0.8), + 9: PupilFeatures.PupilDiameter(0.7) + } + + pupil_diameter_analyzer = WorkloadIndex.PupilDiameterAnalyzer(reference=PupilFeatures.PupilDiameter(1.), period=3) + ts_analysis = pupil_diameter_analyzer.browse(PupilFeatures.TimeStampedPupilDiameters(ts_pupil_diameters)) + + # Check result size + self.assertEqual(len(ts_analysis), 3) + + # Check each workload index + ts_1, analysis_1 = ts_analysis.pop_first() + self.assertTrue(math.isclose(analysis_1, 0.1, abs_tol=1e-2)) + + ts_2, analysis_2 = ts_analysis.pop_first() + self.assertTrue(math.isclose(analysis_2, 0.2, abs_tol=1e-2)) + + ts_3, analysis_3 = ts_analysis.pop_first() + self.assertTrue(math.isclose(analysis_3, 0.1, abs_tol=1e-2)) + +if __name__ == '__main__': + + unittest.main() \ No newline at end of file diff --git a/src/argaze.test/PupilAnalysis/__init__.py b/src/argaze.test/PupilAnalysis/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/argaze.test/PupilFeatures.py b/src/argaze.test/PupilFeatures.py new file mode 100644 index 0000000..6f3d03b --- /dev/null +++ b/src/argaze.test/PupilFeatures.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python + +import unittest + +from argaze import PupilFeatures + +import numpy + +def random_pupil_diameters(size): + """ Generate random TimeStampedPupilDiameters for testing purpose. + Timestamps are current time. + PupilDiameters are random values. + """ + + import random + import time + + ts_pupil_diameters = PupilFeatures.TimeStampedPupilDiameters() + + for i in range(0, size): + + # Edit pupil diameter + random_pupil_diameter = PupilFeatures.PupilDiameter(random.random()) + + # Store pupil diameter + ts_pupil_diameters[time.time()] = random_pupil_diameter + + return ts_pupil_diameters + +class TestPupilDiameterClass(unittest.TestCase): + """Test PupilDiameter class.""" + + def test_new(self): + """Test PupilDiameter creation.""" + + # Check empty PupilDiameter + empty_pupil_diameter = PupilFeatures.PupilDiameter() + + self.assertEqual(empty_pupil_diameter.value, 0.) + self.assertEqual(empty_pupil_diameter.valid, False) + + # Check float PupilDiameter + float_pupil_diameter = PupilFeatures.PupilDiameter(1.23) + + self.assertEqual(float_pupil_diameter.value, 1.23) + self.assertEqual(float_pupil_diameter.valid, True) + + def test_properties(self): + """Test PupilDiameter properties cannot be modified after creation.""" + + pupil_diameter = PupilFeatures.PupilDiameter() + + # Check that pupil diameter value setting fails + with self.assertRaises(AttributeError): + + pupil_diameter.value = 123 + + self.assertNotEqual(pupil_diameter.value, 123) + self.assertEqual(pupil_diameter.value, 0.) + + def test___repr__(self): + """Test PupilDiameter string representation.""" + + # Check empty PupilDiameter representation + self.assertEqual(repr(PupilFeatures.PupilDiameter()), "{\"value\": 0.0}") + +class TestUnvalidPupilDiameterClass(unittest.TestCase): + """Test UnvalidPupilDiameter class.""" + + def test_new(self): + """Test UnvalidPupilDiameter creation.""" + + unvalid_pupil_diameter = PupilFeatures.UnvalidPupilDiameter() + + self.assertEqual(unvalid_pupil_diameter.value, 0.) + self.assertEqual(unvalid_pupil_diameter.valid, False) + + def test___repr__(self): + """Test UnvalidPupilDiameter string representation.""" + + self.assertEqual(repr(PupilFeatures.UnvalidPupilDiameter()), "{\"message\": null, \"value\": 0.0}") + +class TestTimeStampedPupilDiametersClass(unittest.TestCase): + """Test TimeStampedPupilDiameters class.""" + + def test___setitem__(self): + """Test __setitem__ method.""" + + ts_pupil_diameters = PupilFeatures.TimeStampedPupilDiameters() + ts_pupil_diameters[0] = PupilFeatures.PupilDiameter() + ts_pupil_diameters[1] = PupilFeatures.UnvalidPupilDiameter() + ts_pupil_diameters[2] = {"value": 1.23} + + # Check PupilDiameter is correctly stored and accessible as a PupilDiameter + self.assertIsInstance(ts_pupil_diameters[0], PupilFeatures.PupilDiameter) + self.assertEqual(ts_pupil_diameters[0].valid, False) + + # Check UnvalidPupilDiameter is correctly stored and accessible as a UnvalidPupilDiameter + self.assertIsInstance(ts_pupil_diameters[1], PupilFeatures.UnvalidPupilDiameter) + self.assertEqual(ts_pupil_diameters[1].valid, False) + + # Check dict with "value" and "precision" keys is correctly stored and accessible as a PupilDiameter + self.assertIsInstance(ts_pupil_diameters[2], PupilFeatures.PupilDiameter) + self.assertEqual(ts_pupil_diameters[2].valid, True) + + # Check that bad data type insertion fails + with self.assertRaises(AssertionError): + + ts_pupil_diameters[3] = "This string is not a pupil diameter value." + + # Check that dict with bad keys insertion fails + with self.assertRaises(AssertionError): + + ts_pupil_diameters[4] = {"bad_key": 0.} + + def test___repr__(self): + """Test inherited string representation.""" + + ts_pupil_diameters = PupilFeatures.TimeStampedPupilDiameters() + + self.assertEqual(repr(PupilFeatures.TimeStampedPupilDiameters()), "{}") + + ts_pupil_diameters[0] = PupilFeatures.PupilDiameter() + + self.assertEqual(repr(ts_pupil_diameters), "{\"0\": {\"value\": 0.0}}") + + ts_pupil_diameters[0] = PupilFeatures.UnvalidPupilDiameter() + + self.assertEqual(repr(ts_pupil_diameters), "{\"0\": {\"message\": null, \"value\": 0.0}}") + + def test_as_dataframe(self): + """Test inherited as_dataframe method.""" + + ts_pupil_diameters_dataframe = random_pupil_diameters(10).as_dataframe() + + # Check dataframe conversion + self.assertEqual(ts_pupil_diameters_dataframe.index.name, "timestamp") + self.assertEqual(ts_pupil_diameters_dataframe.index.size, 10) + + self.assertEqual(ts_pupil_diameters_dataframe.columns.size, 1) + self.assertEqual(ts_pupil_diameters_dataframe.columns[0], "value") + + self.assertEqual(ts_pupil_diameters_dataframe["value"].dtype, 'float64') + + # Check unvalid diameter conversion + ts_pupil_diameters = PupilFeatures.TimeStampedPupilDiameters() + ts_pupil_diameters[0] = PupilFeatures.UnvalidPupilDiameter() + ts_pupil_diameters_dataframe = ts_pupil_diameters.as_dataframe() + + self.assertEqual(ts_pupil_diameters_dataframe.index.name, "timestamp") + self.assertEqual(ts_pupil_diameters_dataframe.index.size, 1) + + self.assertEqual(ts_pupil_diameters_dataframe.columns.size, 1) + self.assertEqual(ts_pupil_diameters_dataframe.columns[0], "value") + + self.assertEqual(ts_pupil_diameters_dataframe["value"].dtype, 'float64') + +if __name__ == '__main__': + + unittest.main() \ No newline at end of file diff --git a/src/argaze/PupilAnalysis/README.md b/src/argaze/PupilAnalysis/README.md new file mode 100644 index 0000000..3084c15 --- /dev/null +++ b/src/argaze/PupilAnalysis/README.md @@ -0,0 +1,5 @@ +Class interface to work with various gaze analysis algorithms. + +## Wiki + +Read [ArGaze wiki page dedicated to gaze analysis](https://git.recherche.enac.fr/projects/argaze/wiki/Gaze_analysis) submodule. \ No newline at end of file diff --git a/src/argaze/PupilAnalysis/WorkloadIndex.py b/src/argaze/PupilAnalysis/WorkloadIndex.py new file mode 100644 index 0000000..5ed5bee --- /dev/null +++ b/src/argaze/PupilAnalysis/WorkloadIndex.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python + +from typing import TypeVar +from dataclasses import dataclass, field +import math + +from argaze import PupilFeatures + +import numpy + +@dataclass +class PupilDiameterAnalyzer(PupilFeatures.PupilDiameterAnalyzer): + """Periodic average of pupil diameter variations to pupil diameter reference value.""" + + reference: PupilFeatures.PupilDiameter + """ """ + + period: int | float = field(default=1) + """Identification period length.""" + + def __post_init__(self): + + assert(self.reference.valid) + + self.__variations_sum = 0. + self.__variations_number = 0 + self.__last_ts = 0 + + def analyze(self, ts, pupil_diameter) -> float: + """Analyze workload index from successive timestamped pupil diameters.""" + + # Ignore non valid pupil diameter + if not pupil_diameter.valid: + + return None + + if ts - self.__last_ts >= self.period: + + workload_index = (self.__variations_sum / self.__variations_number) / self.reference.value + + self.__variations_sum = abs(pupil_diameter.value - self.reference.value) + self.__variations_number = 1 + self.__last_ts = ts + + return workload_index + + else: + + self.__variations_sum += abs(pupil_diameter.value - self.reference.value) + self.__variations_number += 1 + \ No newline at end of file diff --git a/src/argaze/PupilAnalysis/__init__.py b/src/argaze/PupilAnalysis/__init__.py new file mode 100644 index 0000000..4a5586f --- /dev/null +++ b/src/argaze/PupilAnalysis/__init__.py @@ -0,0 +1,5 @@ +""" +.. include:: README.md +""" +__docformat__ = "restructuredtext" +__all__ = ['ReferenceBasedPupilDiameterVariationIdentifier'] \ No newline at end of file diff --git a/src/argaze/PupilFeatures.py b/src/argaze/PupilFeatures.py new file mode 100644 index 0000000..94eaa07 --- /dev/null +++ b/src/argaze/PupilFeatures.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python + +from typing import TypeVar +from dataclasses import dataclass, field +import json + +from argaze import DataStructures + +@dataclass(frozen=True) +class PupilDiameter(): + """Define pupil diameter as ...""" + + value: float = field(default=0.) + """Pupil diameter value.""" + + @property + def valid(self) -> bool: + """Is the value not 0""" + + return self.value != 0. + + def __repr__(self): + """String representation""" + + return json.dumps(self, ensure_ascii = False, default=vars) + +class UnvalidPupilDiameter(PupilDiameter): + """Unvalid pupil diameter.""" + + def __init__(self, message=None): + + self.message = message + + super().__init__(0.) + +TimeStampedPupilDiametersType = TypeVar('TimeStampedPupilDiameters', bound="TimeStampedPupilDiameters") +# Type definition for type annotation convenience + +class TimeStampedPupilDiameters(DataStructures.TimeStampedBuffer): + """Define timestamped buffer to store pupil diameters.""" + + def __setitem__(self, key, value: PupilDiameter|dict): + """Force PupilDiameter storage.""" + + # Convert dict into PupilDiameter + if type(value) == dict: + + assert(set(['value']).issubset(value.keys())) + + if 'message' in value.keys(): + + value = UnvalidPupilDiameter(value['message']) + + else: + + value = PupilDiameter(value['value']) + + assert(type(value) == PupilDiameter or type(value) == UnvalidPupilDiameter) + + super().__setitem__(key, value) + + @classmethod + def from_json(self, json_filepath: str) -> TimeStampedPupilDiametersType: + """Create a TimeStampedPupilDiametersType from .json file.""" + + with open(json_filepath, encoding='utf-8') as ts_buffer_file: + + json_buffer = json.load(ts_buffer_file) + + return TimeStampedPupilDiameters({ast.literal_eval(ts_str): json_buffer[ts_str] for ts_str in json_buffer}) + +TimeStampedBufferType = TypeVar('TimeStampedBuffer', bound="TimeStampedBuffer") +# Type definition for type annotation convenience + +class PupilDiameterAnalyzer(): + """Abstract class to define what should provide a pupil diameter analyser.""" + + def analyze(self, ts, pupil_diameter) -> float: + """Analyse pupil diameter from successive timestamped pupil diameters.""" + + raise NotImplementedError('analyze() method not implemented') + + def browse(self, ts_pupil_diameters: TimeStampedPupilDiameters) -> TimeStampedBufferType: + """Analyze by browsing timestamped pupil diameters.""" + + assert(type(ts_pupil_diameters) == TimeStampedPupilDiameters) + + ts_analyzis = DataStructures.TimeStampedBuffer() + + # Iterate on pupil diameters + for ts, pupil_diameter in ts_pupil_diameters.items(): + + analysis = self.analyze(ts, pupil_diameter) + + if analysis is not None: + + ts_analyzis[ts] = analysis + + return ts_analyzis diff --git a/src/argaze/__init__.py b/src/argaze/__init__.py index e55cf92..0099bfc 100644 --- a/src/argaze/__init__.py +++ b/src/argaze/__init__.py @@ -2,4 +2,4 @@ .. include:: ../../README.md """ __docformat__ = "restructuredtext" -__all__ = ['ArUcoMarkers','AreaOfInterest','ArFeatures','GazeFeatures','GazeAnalysis','DataStructures','utils'] \ No newline at end of file +__all__ = ['ArUcoMarkers','AreaOfInterest','ArFeatures','GazeFeatures','GazeAnalysis','PupilFeatures','PupilAnalysis','DataStructures','utils'] \ No newline at end of file diff --git a/src/argaze/utils/environment_edit.py b/src/argaze/utils/environment_edit.py new file mode 100644 index 0000000..ae45769 --- /dev/null +++ b/src/argaze/utils/environment_edit.py @@ -0,0 +1,343 @@ +#!/usr/bin/env python + +import argparse +import time + +from argaze import ArFeatures, GazeFeatures +from argaze.AreaOfInterest import AOIFeatures +from argaze.ArUcoMarkers import ArUcoScene +from argaze.utils import MiscFeatures + +from tobiiproglasses2 import * + +import cv2 +import numpy + +def main(): + """ + Load AR environment from .json file, detect ArUco markers into movie frames and estimate environment pose. + Edit environment setup to improve pose estimation. + """ + + # Manage arguments + parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0]) + parser.add_argument('environment', metavar='ENVIRONMENT', type=str, help='ar environment filepath') + parser.add_argument('movie', metavar='MOVIE', type=str, default=None, help='movie path') + parser.add_argument('-s','--start', metavar='START', type=float, default=None, help='start time in second') + parser.add_argument('-o', '--output', metavar='OUT', type=str, default='environment.json', help='edited ar environment file path') + args = parser.parse_args() + + # Load AR enviroment + ar_environment = ArFeatures.ArEnvironment.from_json(args.environment) + + #print('ArEnvironment:\n', ar_environment) + + # Select first AR scene + ar_scene = list(ar_environment.scenes.values())[0] + + # Create a window to display AR environment + cv2.namedWindow(ar_environment.name, cv2.WINDOW_AUTOSIZE) + + # Init mouse interaction + pointer = (0, 0) + left_click = (0, 0) + right_click = (0, 0) + right_drag = (0, 0) + right_button = False + edit_trans = False # translate + edit_z = False + + # Update pointer position + def on_mouse_event(event, x, y, flags, param): + + nonlocal pointer + nonlocal left_click + nonlocal right_click + nonlocal right_drag + nonlocal right_button + + # Update pointer + pointer = (x, y) + + # Update left_click + if event == cv2.EVENT_LBUTTONUP: + + left_click = pointer + + # Udpate right_button + elif event == cv2.EVENT_RBUTTONDOWN and not right_button: + + right_button = True + right_click = pointer + + elif event == cv2.EVENT_RBUTTONUP and right_button: + + right_button = False + + # Udpate right_drag + if right_button: + + right_drag = (pointer[0] - right_click[0], pointer[1] - right_click[1]) + + # Attach mouse callback to window + cv2.setMouseCallback(ar_environment.name, on_mouse_event) + + # Enable movie video capture + video_capture = cv2.VideoCapture(args.movie) + + video_fps = video_capture.get(cv2.CAP_PROP_FPS) + frame_width = int(video_capture.get(cv2.CAP_PROP_FRAME_WIDTH)) + frame_height = int(video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT)) + + # Enable exit signal handler + exit = MiscFeatures.ExitSignalHandler() + + # Init frame selection + current_frame_index = -1 + _, current_frame = video_capture.read() + next_frame_index = int(args.start * video_fps) + refresh_detection = False + + # Init marker selection + selected_marker_id = -1 + + # Init place edition + place_edit = {} + + while not exit.status(): + + # Edit fake gaze position from pointer + gaze_position = GazeFeatures.GazePosition(pointer, precision=2) + + # Select a new frame and detect markers once + if next_frame_index != current_frame_index or refresh_detection: + + video_capture.set(cv2.CAP_PROP_POS_FRAMES, next_frame_index) + + success, video_frame = video_capture.read() + + if success: + + current_frame_index = video_capture.get(cv2.CAP_PROP_POS_FRAMES) - 1 + current_frame_time = video_capture.get(cv2.CAP_PROP_POS_MSEC) + + # Detect markers + ar_environment.aruco_detector.detect_markers(video_frame) + + # Draw detected markers + ar_environment.aruco_detector.draw_detected_markers(video_frame) + + # Draw focus area + cv2.rectangle(video_frame, (int(frame_width/6), 0), (int(frame_width*(1-1/6)), int(frame_height)), (255, 150, 150), 1) + + # Write timing + cv2.rectangle(video_frame, (0, 0), (700, 50), (63, 63, 63), -1) + cv2.putText(video_frame, f'Time: {int(current_frame_time)} ms', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) + + # Copy frame + current_frame = video_frame.copy() + + # Keep last frame + else: + + video_frame = current_frame.copy() + + # Draw detected markers + ar_environment.aruco_detector.draw_detected_markers(video_frame) + + # Write detected marker ids + cv2.putText(video_frame, f'Detected markers: {list(ar_environment.aruco_detector.detected_markers.keys())}', (20, frame_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) + + # Handle marker selection on left click + if len(ar_environment.aruco_detector.detected_markers) > 0: + + # Update selected marker id by left clicking on marker + for (marker_id, marker) in ar_environment.aruco_detector.detected_markers.items(): + + marker_aoi = marker.corners.reshape(4, 2).view(AOIFeatures.AreaOfInterest) + + if marker_aoi.contains_point(left_click): + + selected_marker_id = marker_id + + # If a marker is selected + try: + + # Retreive selected marker + selected_marker = ar_environment.aruco_detector.detected_markers[selected_marker_id] + + # Estimate selected marker pose + ar_environment.aruco_detector.estimate_markers_pose([selected_marker_id]) + + # Retreive selected marker place + selected_place = ar_scene.aruco_scene.places[selected_marker_id] + + # On right click + if right_button: + + pointer_delta_x, pointer_delta_y = right_drag[0] / frame_width, right_drag[1] / frame_height + + place_edit[selected_marker_id] = {'rotation': (0, 0, 0), 'translation': (0, 0, 0)} + + if edit_trans: + + # Edit place rotation + if edit_z: + place_edit[selected_marker_id]['rotation'] = (0, 0, -pointer_delta_y) + else: + place_edit[selected_marker_id]['rotation'] = (pointer_delta_y, pointer_delta_x, 0) + + else: + + # Edit place translation + if edit_z: + place_edit[selected_marker_id]['translation'] = (0, 0, pointer_delta_y) + else: + place_edit[selected_marker_id]['translation'] = (-pointer_delta_x, pointer_delta_y, 0) + + # Apply transformations + R = selected_place.rotation.dot(ArUcoScene.make_rotation_matrix(*place_edit[selected_marker_id]['rotation']).T) + T = selected_place.translation + numpy.array(place_edit[selected_marker_id]['translation']) + + edited_place = ArUcoScene.Place(T, R, selected_marker) + + else: + + edited_place = selected_place + + cv2.rectangle(video_frame, (0, 130), (460, 450), (127, 127, 127), -1) + + # Write edited rotation matrix + R = edited_place.rotation + cv2.putText(video_frame, f'Rotation matrix:', (20, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'{R[0][0]:.3f} {R[0][1]:.3f} {R[0][2]:.3f}', (40, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'{R[1][0]:.3f} {R[1][1]:.3f} {R[1][2]:.3f}', (40, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'{R[2][0]:.3f} {R[2][1]:.3f} {R[2][2]:.3f}', (40, 280), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + + # Write edited translation vector + T = edited_place.translation + cv2.putText(video_frame, f'Translation vector:', (20, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'{T[0]:.3f}', (40, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'{T[1]:.3f}', (40, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'{T[2]:.3f}', (40, 440), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + + # Replace selected place by edited place + ar_scene.aruco_scene.places[selected_marker_id] = edited_place + + # Estimate scene pose considering only selected marker + tvec, rmat, _ = ar_scene.estimate_pose({selected_marker_id: selected_marker}) + + # Draw expected marker places + ar_scene.draw_places(video_frame) + + # Project AOI scene into frame according estimated pose + aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV) + + # Draw AOI scene projection with gaze + aoi_scene_projection.draw_circlecast(video_frame, gaze_position) + + # Catch missing selected marker + except KeyError: + + # Write error + if selected_marker_id >= 0: + + cv2.putText(video_frame, f'Marker {selected_marker_id} not found', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + + # Catch exceptions raised by estimate_pose and project methods + except (ArFeatures.PoseEstimationFailed, ArFeatures.SceneProjectionFailed) as e: + + cv2.rectangle(video_frame, (0, 50), (700, 100), (127, 127, 127), -1) + cv2.putText(video_frame, f'Error: {e}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) + + # Draw frame + cv2.imshow(ar_environment.name, video_frame) + + # Draw pointer + gaze_position.draw(video_frame) + + # Write selected marker id + if selected_marker_id >= 0: + + cv2.rectangle(video_frame, (0, 50), (550, 90), (127, 127, 127), -1) + + # Select color + if edit_z: + str_axis = 'Z' + color_axis = (255, 0, 0) + else: + str_axis = 'XY' + color_axis = (0, 255, 255) + + if edit_trans: + cv2.putText(video_frame, f'Rotate marker {selected_marker_id} around axis {str_axis}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, color_axis, 1, cv2.LINE_AA) + else: + cv2.putText(video_frame, f'Translate marker {selected_marker_id} along axis {str_axis}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, color_axis, 1, cv2.LINE_AA) + + # Write documentation + else: + cv2.rectangle(video_frame, (0, 50), (650, 250), (127, 127, 127), -1) + cv2.putText(video_frame, f'> Left click on marker: select scene', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'> T: translate, R: rotate', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'> Z: switch Z axis edition', (20, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'> Right click and drag: edit XY axis', (20, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) + cv2.putText(video_frame, f'> Ctrl + S: save scene', (20, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) + + # Reset left_click + left_click = (0, 0) + + key_pressed = cv2.waitKey(10) + + #if key_pressed != -1: + # print(key_pressed) + + # Select previous frame with left arrow + if key_pressed == 2: + next_frame_index -= 1 + + # Select next frame with right arrow + if key_pressed == 3: + next_frame_index += 1 + + # Clip frame index + if next_frame_index < 0: + next_frame_index = 0 + + # Edit rotation with r key + if key_pressed == 114: + edit_trans = True + + # Edit translation with t key + if key_pressed == 116: + edit_trans = False + + # Switch Z axis edition + if key_pressed == 122: + edit_z = not edit_z + + # Save selected marker edition using 'Ctrl + s' + if key_pressed == 19: + ar_environment.to_json(args.output) + print(f'Environment saved into {args.output}') + + # Close window using 'Esc' key + if key_pressed == 27: + break + + # Reload detector configuration on 'c' key + if key_pressed == 99: + print(f'TODO: Reload ArUcoDetector parameters') + refresh_detection = True + + # Display video + cv2.imshow(ar_environment.name, video_frame) + + # Close movie capture + video_capture.release() + + # Stop frame display + cv2.destroyAllWindows() + +if __name__ == '__main__': + + main() \ No newline at end of file -- cgit v1.1