aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArFeatures.py667
1 files changed, 370 insertions, 297 deletions
diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py
index 3e1a56f..62ce4d8 100644
--- a/src/argaze/ArFeatures.py
+++ b/src/argaze/ArFeatures.py
@@ -63,10 +63,10 @@ class ArEnvironment():
scene._environment = self
# Init AOI scene projections
- self.__aoi_2d_scenes = {}
+ self.__camera_frames = {}
# Init a lock to share AOI scene projections between multiple threads
- self.__aoi_2d_scenes_lock = threading.Lock()
+ self.__camera_frames_lock = threading.Lock()
@classmethod
def from_json(self, json_filepath: str) -> ArSceneType:
@@ -165,11 +165,16 @@ class ArEnvironment():
new_aoi_3d_scene = AOI3DScene.AOI3DScene(aoi_3d_scene_value)
- # Build frames
- new_frames = {}
- for frame_name, frame_data in scene_data.pop('frames').items():
+ # Define frame data processor
+ def frame_data_processor(frame_data, force_frame_size: list = []) -> ArFrame:
- new_frame_size = frame_data.pop('size')
+ if len(force_frame_size) == 2:
+
+ new_frame_size = force_frame_size
+
+ else:
+
+ new_frame_size = frame_data.pop('size')
# Load background image
try:
@@ -190,11 +195,11 @@ class ArEnvironment():
gaze_movement_identifier_type, gaze_movement_identifier_parameters = gaze_movement_identifier_value.popitem()
gaze_movement_identifier_module = importlib.import_module(f'argaze.GazeAnalysis.{gaze_movement_identifier_type}')
- gaze_movement_identifier = gaze_movement_identifier_module.GazeMovementIdentifier(**gaze_movement_identifier_parameters)
+ new_gaze_movement_identifier = gaze_movement_identifier_module.GazeMovementIdentifier(**gaze_movement_identifier_parameters)
except KeyError:
- gaze_movement_identifier = None
+ new_gaze_movement_identifier = None
# Load scan path analyzers
new_scan_path_analyzers = {}
@@ -284,11 +289,30 @@ class ArEnvironment():
pass
- # Append new frame
- new_frames[frame_name] = ArFrame.from_scene(new_aoi_3d_scene, frame_name, new_frame_size, new_frame_background, gaze_movement_identifier, new_scan_path_analyzers, new_aoi_scan_path_analyzers, **frame_data)
+ return new_frame_size, new_frame_background, new_gaze_movement_identifier, new_scan_path_analyzers, new_aoi_scan_path_analyzers
+
+ # Build camera frame as large as aruco dectector optic parameters
+ try:
+
+ camera_frame_data = scene_data.pop('camera_frame')
+ new_frame_size, new_frame_background, new_gaze_movement_identifier, new_scan_path_analyzers, new_aoi_scan_path_analyzers = frame_data_processor(camera_frame_data, force_frame_size=new_optic_parameters.dimensions)
+ new_camera_frame = ArFrame.from_scene(new_aoi_3d_scene, None, new_frame_size, new_frame_background, new_gaze_movement_identifier, new_scan_path_analyzers, new_aoi_scan_path_analyzers, **camera_frame_data)
+
+ except KeyError:
+
+ new_camera_frame = None #ArFrame.from_scene(new_aoi_3d_scene, None, new_optic_parameters.dimensions)
+
+ # Build AOI frames
+ new_aoi_frames = {}
+ for aoi_name, aoi_frame_data in scene_data.pop('aoi_frames').items():
+
+ new_frame_size, new_frame_background, new_gaze_movement_identifier, new_scan_path_analyzers, new_aoi_scan_path_analyzers = frame_data_processor(aoi_frame_data)
+
+ # Append new AOI frame
+ new_aoi_frames[aoi_name] = ArFrame.from_scene(new_aoi_3d_scene, aoi_name, new_frame_size, new_frame_background, new_gaze_movement_identifier, new_scan_path_analyzers, new_aoi_scan_path_analyzers, **aoi_frame_data)
# Append new scene
- new_scenes[scene_name] = ArScene(new_aruco_scene, new_aoi_3d_scene, new_frames, **scene_data)
+ new_scenes[scene_name] = ArScene(new_aruco_scene, new_aoi_3d_scene, new_camera_frame, new_aoi_frames, **scene_data)
return ArEnvironment(new_name, new_aruco_detector, new_scenes)
@@ -313,26 +337,25 @@ class ArEnvironment():
# For each scene
for scene_name, scene in self.scenes.items():
- # For each frame
- for frame_name, frame in scene.frames.items():
+ # For each aoi frame
+ for frame_name, frame in scene.aoi_frames.items():
yield scene_name, frame_name, frame
def detect_and_project(self, image: numpy.array) -> dict:
- """Detect environment aruco markers from image and project scenes."""
+ """Detect environment aruco markers from image and project scenes into camera frame."""
# Detect aruco markers
self.aruco_detector.detect_markers(image)
- # Project each AOI scene
- new_aoi_2d_scenes = {}
+ # Project each aoi 3d scene into camera frame
for scene_name, scene in self.scenes.items():
# Project scene
try:
# Try to build AOI scene from detected ArUco marker corners
- new_aoi_2d_scenes[scene_name] = scene.build_aruco_aoi_scene(self.aruco_detector.detected_markers)
+ scene.build_aruco_aoi_scene(self.aruco_detector.detected_markers)
except SceneProjectionFailed:
@@ -342,63 +365,16 @@ class ArEnvironment():
# Estimate scene pose from detected scene markers
tvec, rmat, _, _ = scene.estimate_pose(self.aruco_detector.detected_markers)
- # Project AOI scene into video image according estimated pose
- new_aoi_2d_scenes[scene_name] = scene.project(tvec, rmat)
-
- # Lock scene projections exploitation
- self.__aoi_2d_scenes_lock.acquire()
-
- # Copy scene projections
- self.__aoi_2d_scenes = new_aoi_2d_scenes.copy()
-
- # Unlock scene projections exploitation
- self.__aoi_2d_scenes_lock.release()
+ # Project AOI scene into camera frame according estimated pose
+ scene.project(tvec, rmat)
- def look(self, timestamp: int|float, gaze_position: GazeFeatures.GazePosition, data_generator: bool = False):
- """Project timestamped gaze position into current scene projections."""
-
- # Can't use scene projection when it is locked
- if self.__aoi_2d_scenes_lock.locked():
-
- #TODO: Store ignored timestamped gaze positions for further projections
- print('Ignoring ', timestamp, gaze_position)
- return
-
- # Lock scene projections
- self.__aoi_2d_scenes_lock.acquire()
+ def look(self, timestamp: int|float, gaze_position: GazeFeatures.GazePosition):
+ """Project timestamped gaze position into each scene."""
# For each aoi scene projection
for scene_name, scene in self.scenes.items():
- try:
-
- aoi_2d_scene = self.__aoi_2d_scenes[scene_name]
-
- # For each scene frames
- for frame_name, frame in scene.frames.items():
-
- # TODO: Add option to use gaze precision circle
- if aoi_2d_scene[frame.name].contains_point(gaze_position.value):
-
- inner_x, inner_y = self.__aoi_2d_scenes[scene_name][frame.name].clockwise().inner_axis(gaze_position.value)
-
- # QUESTION: How to project gaze precision?
- inner_gaze_position = GazeFeatures.GazePosition((inner_x, inner_y))
-
- gaze_movement, look_at, scan_step_analysis, aoi_scan_step_analysis = frame.look(timestamp, inner_gaze_position * frame.size)
-
- # Generate looking data
- if data_generator:
-
- yield scene_name, frame_name, frame, gaze_movement, look_at, scan_step_analysis, aoi_scan_step_analysis
-
- # Ignore missing aoi scene projection
- except KeyError:
-
- pass
-
- # Unlock scene projections
- self.__aoi_2d_scenes_lock.release()
+ scene.look(timestamp, gaze_position)
def to_json(self, json_filepath):
"""Save environment to .json file."""
@@ -413,11 +389,10 @@ class ArEnvironment():
# Draw detected markers
self.aruco_detector.draw_detected_markers(image)
- # Draw each AOI scene
- for scene_name, aoi_2d_scene in self.__aoi_2d_scenes.items():
+ # Draw each scene
+ for scene_name, scene in self.scenes.items():
- # Draw AOI scene projection
- aoi_2d_scene.draw(image, color=(255, 255, 255))
+ scene.draw(image)
class PoseEstimationFailed(Exception):
"""
@@ -440,228 +415,6 @@ class SceneProjectionFailed(Exception):
super().__init__(message)
@dataclass
-class ArScene():
- """
- Define an Augmented Reality scene with ArUco markers and AOI scenes.
-
- Parameters:
- aruco_scene: ArUco markers 3D scene description used to estimate scene pose from detected markers: see [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function below.
-
- aoi_3d_scene: AOI 3D scene description that will be projected onto estimated scene once its pose will be estimated : see [project][argaze.ArFeatures.ArScene.project] function below.
-
- frames: All scene frames
-
- aruco_axis: Optional dictionary to define orthogonal axis where each axis is defined by list of 3 markers identifier (first is origin). \
- This pose estimation strategy is used by [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function when at least 3 markers are detected.
-
- aruco_aoi: Optional dictionary of AOI defined by list of markers identifier and markers corners index tuples: see [build_aruco_aoi_scene][argaze.ArFeatures.ArScene.build_aruco_aoi_scene] function below.
-
- angle_tolerance: Optional angle error tolerance to validate marker pose in degree used into [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function.
-
- distance_tolerance: Optional distance error tolerance to validate marker pose in centimeter used into [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function.
- """
-
- aruco_scene: ArUcoScene.ArUcoScene = field(default_factory=ArUcoScene.ArUcoScene)
- aoi_3d_scene: AOI3DScene.AOI3DScene = field(default_factory=AOI3DScene.AOI3DScene)
- frames: dict = field(default_factory=dict)
- aruco_axis: dict = field(default_factory=dict)
- aruco_aoi: dict = field(default_factory=dict)
- angle_tolerance: float = field(default=0.)
- distance_tolerance: float = field(default=0.)
-
- def __post_init__(self):
-
- # Define environment attribute: it will be setup by parent environment later
- self._environment = None
-
- # Preprocess orthogonal projection to speed up further aruco aoi processings
- self.__orthogonal_projection_cache = self.aoi_3d_scene.orthogonal_projection
-
- # Setup frames scene after frame creation
- for name, frame in self.frames.items():
- frame._scene = self
-
- def __str__(self) -> str:
- """
- Returns:
- String representation
- """
-
- output = f'ArEnvironment:\n{self._environment.name}\n'
- output += f'ArUcoScene:\n{self.aruco_scene}\n'
- output += f'AOI3DScene:\n{self.aoi_3d_scene}\n'
-
- return output
-
- def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, str, dict]:
- """Estimate scene pose from detected ArUco markers.
-
- Returns:
- scene translation vector
- scene rotation matrix
- pose estimation strategy
- dict of markers used to estimate the pose
- """
-
- # Pose estimation fails when no marker is detected
- if len(detected_markers) == 0:
-
- raise PoseEstimationFailed('No marker detected')
-
- scene_markers, _ = self.aruco_scene.filter_markers(detected_markers)
-
- # Pose estimation fails when no marker belongs to the scene
- if len(scene_markers) == 0:
-
- raise PoseEstimationFailed('No marker belongs to the scene')
-
- # Estimate scene pose from unique marker transformations
- elif len(scene_markers) == 1:
-
- marker_id, marker = scene_markers.popitem()
- tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(marker)
-
- return tvec, rmat, 'estimate_pose_from_single_marker', {marker_id: marker}
-
- # Try to estimate scene pose from 3 markers defining an orthogonal axis
- elif len(scene_markers) >= 3 and len(self.aruco_axis) > 0:
-
- for axis_name, axis_markers in self.aruco_axis.items():
-
- try:
-
- origin_marker = scene_markers[axis_markers['origin_marker']]
- horizontal_axis_marker = scene_markers[axis_markers['horizontal_axis_marker']]
- vertical_axis_marker = scene_markers[axis_markers['vertical_axis_marker']]
-
- tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker)
-
- return tvec, rmat, 'estimate_pose_from_axis_markers', {origin_marker.identifier: origin_marker, horizontal_axis_marker.identifier: horizontal_axis_marker, vertical_axis_marker.identifier: vertical_axis_marker}
-
- except:
- pass
-
- raise PoseEstimationFailed('No marker axis')
-
- # Otherwise, check markers consistency
- consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.check_markers_consistency(scene_markers, self.angle_tolerance, self.distance_tolerance)
-
- # Pose estimation fails when no marker passes consistency checking
- if len(consistent_markers) == 0:
-
- raise PoseEstimationFailed('Unconsistent marker poses', unconsistencies)
-
- # Otherwise, estimate scene pose from all consistent markers pose
- tvec, rmat = self.aruco_scene.estimate_pose_from_markers(consistent_markers)
-
- return tvec, rmat, 'estimate_pose_from_markers', consistent_markers
-
- def project(self, tvec: numpy.array, rvec: numpy.array, visual_hfov: float = 0.) -> AOI2DScene.AOI2DScene:
- """Project AOI scene according estimated pose and optional horizontal field of view clipping angle.
-
- Parameters:
- tvec: translation vector
- rvec: rotation vector
- visual_hfov: horizontal field of view clipping angle
- """
-
- # Clip AOI out of the visual horizontal field of view (optional)
- if visual_hfov > 0:
-
- # Transform scene into camera referential
- aoi_3d_scene_camera_ref = self.aoi_3d_scene.transform(tvec, rvec)
-
- # Get aoi inside vision cone field
- cone_vision_height_cm = 200 # cm
- cone_vision_radius_cm = numpy.tan(numpy.deg2rad(visual_hfov / 2)) * cone_vision_height_cm
-
- _, aoi_outside = aoi_3d_scene_camera_ref.vision_cone(cone_vision_radius_cm, cone_vision_height_cm)
-
- # Keep only aoi inside vision cone field
- aoi_3d_scene_copy = self.aoi_3d_scene.copy(exclude=aoi_outside.keys())
-
- else:
-
- aoi_3d_scene_copy = self.aoi_3d_scene.copy()
-
- aoi_2d_scene = aoi_3d_scene_copy.project(tvec, rvec, self._environment.aruco_detector.optic_parameters.K)
-
- # Warn user when the projected scene is empty
- if len(aoi_2d_scene) == 0:
-
- raise SceneProjectionFailed('AOI projection is empty')
-
- return aoi_2d_scene
-
- def build_aruco_aoi_scene(self, detected_markers) -> AOI2DScene.AOI2DScene:
- """
- Build AOI scene from detected ArUco markers as defined in aruco_aoi dictionary.
-
- Returns:
- built AOI 2D scene
- """
-
- # Check aruco aoi is defined
- if len(self.aruco_aoi) == 0:
-
- raise SceneProjectionFailed('No aruco aoi is defined')
-
- # AOI projection fails when no marker is detected
- if len(detected_markers) == 0:
-
- raise SceneProjectionFailed('No marker detected')
-
- aruco_aoi_scene = {}
-
- for aruco_aoi_name, aoi in self.aruco_aoi.items():
-
- # Each aoi's corner is defined by a marker's corner
- aoi_corners = []
- for corner in ["upper_left_corner", "upper_right_corner", "lower_right_corner", "lower_left_corner"]:
-
- marker_identifier = aoi[corner]["marker_identifier"]
-
- try:
-
- aoi_corners.append(detected_markers[marker_identifier].corners[0][aoi[corner]["marker_corner_index"]])
-
- except Exception as e:
-
- raise SceneProjectionFailed(f'Missing marker #{e} to build ArUco AOI scene')
-
- aruco_aoi_scene[aruco_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners)
-
- # Then each inner aoi is projected from the current aruco aoi
- for inner_aoi_name, inner_aoi in self.aoi_3d_scene.items():
-
- if aruco_aoi_name != inner_aoi_name:
-
- aoi_corners = [numpy.array(aruco_aoi_scene[aruco_aoi_name].outter_axis(inner)) for inner in self.__orthogonal_projection_cache[inner_aoi_name]]
- aruco_aoi_scene[inner_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners)
-
- return AOI2DScene.AOI2DScene(aruco_aoi_scene)
-
- def draw_axis(self, image: numpy.array):
- """
- Draw scene axis into image.
-
- Parameters:
- image: where to draw
- """
-
- self.aruco_scene.draw_axis(image, self._environment.aruco_detector.optic_parameters.K, self._environment.aruco_detector.optic_parameters.D)
-
- def draw_places(self, image: numpy.array):
- """
- Draw scene places into image.
-
- Parameters:
- image: where to draw
- """
-
- self.aruco_scene.draw_places(image, self._environment.aruco_detector.optic_parameters.K, self._environment.aruco_detector.optic_parameters.D)
-
-@dataclass
class ArFrame():
"""
Define Augmented Reality frame as an AOI2DScene made from a projected then reframed parent AOI3DScene.
@@ -702,9 +455,15 @@ class ArFrame():
self.__looking_lock = threading.Lock()
@classmethod
- def from_scene(self, aoi_3d_scene, aoi_name, size, background, gaze_movement_identifier, scan_path_analyzers: list = [], aoi_scan_path_analyzers: list = [], heatmap: bool = False) -> ArFrameType:
+ def from_scene(self, aoi_3d_scene, aoi_name, size, background: numpy.array = numpy.empty((0, 0)), gaze_movement_identifier: GazeFeatures.GazeMovementIdentifier = None, scan_path_analyzers: list = [], aoi_scan_path_analyzers: list = [], heatmap: bool = False) -> ArFrameType:
- aoi_2d_scene = aoi_3d_scene.orthogonal_projection.reframe(aoi_name, size)
+ if aoi_name:
+
+ aoi_2d_scene = aoi_3d_scene.orthogonal_projection.reframe(aoi_name, size)
+
+ else:
+
+ aoi_2d_scene = AOI2DScene.AOI2DScene()
return ArFrame(aoi_name, \
size, \
@@ -885,3 +644,317 @@ class ArFrame():
# Return looking data
return gaze_movement, look_at, scan_step_analysis, aoi_scan_step_analysis
+
+ @property
+ def image(self,):
+ """
+ Get frame image
+ """
+
+ image = self.background.copy()
+
+ self.aoi_2d_scene.draw(image, color=(255, 255, 255))
+ self.current_gaze_position.draw(image, color=(255, 255, 255))
+
+ self.current_gaze_movement.draw(image, color=(0, 255, 255))
+ self.current_gaze_movement.draw_positions(image)
+
+ # Check frame fixation
+ if GazeFeatures.is_fixation(self.current_gaze_movement):
+
+ # Draw looked AOI
+ self.aoi_2d_scene.draw_circlecast(image, self.current_gaze_movement.focus, self.current_gaze_movement.deviation_max, base_color=(0, 0, 0), matching_color=(255, 255, 255))
+
+ return image
+
+@dataclass
+class ArScene():
+ """
+ Define an Augmented Reality scene with ArUco markers and AOI scenes.
+
+ Parameters:
+ aruco_scene: ArUco markers 3D scene description used to estimate scene pose from detected markers: see [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function below.
+
+ aoi_3d_scene: AOI 3D scene description that will be projected onto estimated scene once its pose will be estimated : see [project][argaze.ArFeatures.ArScene.project] function below.
+
+ camera_frame: Where AOI 3D scene will be projected
+
+ aoi_frames: Optional dictionary to define AOI as ArFrame.
+
+ aruco_axis: Optional dictionary to define orthogonal axis where each axis is defined by list of 3 markers identifier (first is origin). \
+ This pose estimation strategy is used by [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function when at least 3 markers are detected.
+
+ aruco_aoi: Optional dictionary of AOI defined by list of markers identifier and markers corners index tuples: see [build_aruco_aoi_scene][argaze.ArFeatures.ArScene.build_aruco_aoi_scene] function below.
+
+ angle_tolerance: Optional angle error tolerance to validate marker pose in degree used into [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function.
+
+ distance_tolerance: Optional distance error tolerance to validate marker pose in centimeter used into [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function.
+ """
+
+ aruco_scene: ArUcoScene.ArUcoScene = field(default_factory=ArUcoScene.ArUcoScene)
+ aoi_3d_scene: AOI3DScene.AOI3DScene = field(default_factory=AOI3DScene.AOI3DScene)
+ camera_frame: ArFrame = field(default_factory=ArFrame)
+ aoi_frames: dict = field(default_factory=dict)
+ aruco_axis: dict = field(default_factory=dict)
+ aruco_aoi: dict = field(default_factory=dict)
+ angle_tolerance: float = field(default=0.)
+ distance_tolerance: float = field(default=0.)
+
+ def __post_init__(self):
+
+ # Define environment attribute: it will be setup by parent environment later
+ self._environment = None
+
+ # Preprocess orthogonal projection to speed up further aruco aoi processings
+ self.__orthogonal_projection_cache = self.aoi_3d_scene.orthogonal_projection
+
+ # Setup ArFrame scene attribute after ArFrame creation
+ for aoi_name, frame in self.aoi_frames.items():
+ frame._scene = self
+
+ # Init lock to share camera frame with multiples threads
+ self.__camera_frame_lock = threading.Lock()
+
+ def __str__(self) -> str:
+ """
+ Returns:
+ String representation
+ """
+
+ output = f'ArEnvironment:\n{self._environment.name}\n'
+ output += f'ArUcoScene:\n{self.aruco_scene}\n'
+ output += f'AOI3DScene:\n{self.aoi_3d_scene}\n'
+
+ return output
+
+ def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, str, dict]:
+ """Estimate scene pose from detected ArUco markers.
+
+ Returns:
+ scene translation vector
+ scene rotation matrix
+ pose estimation strategy
+ dict of markers used to estimate the pose
+ """
+
+ # Pose estimation fails when no marker is detected
+ if len(detected_markers) == 0:
+
+ raise PoseEstimationFailed('No marker detected')
+
+ scene_markers, _ = self.aruco_scene.filter_markers(detected_markers)
+
+ # Pose estimation fails when no marker belongs to the scene
+ if len(scene_markers) == 0:
+
+ raise PoseEstimationFailed('No marker belongs to the scene')
+
+ # Estimate scene pose from unique marker transformations
+ elif len(scene_markers) == 1:
+
+ marker_id, marker = scene_markers.popitem()
+ tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(marker)
+
+ return tvec, rmat, 'estimate_pose_from_single_marker', {marker_id: marker}
+
+ # Try to estimate scene pose from 3 markers defining an orthogonal axis
+ elif len(scene_markers) >= 3 and len(self.aruco_axis) > 0:
+
+ for axis_name, axis_markers in self.aruco_axis.items():
+
+ try:
+
+ origin_marker = scene_markers[axis_markers['origin_marker']]
+ horizontal_axis_marker = scene_markers[axis_markers['horizontal_axis_marker']]
+ vertical_axis_marker = scene_markers[axis_markers['vertical_axis_marker']]
+
+ tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker)
+
+ return tvec, rmat, 'estimate_pose_from_axis_markers', {origin_marker.identifier: origin_marker, horizontal_axis_marker.identifier: horizontal_axis_marker, vertical_axis_marker.identifier: vertical_axis_marker}
+
+ except:
+ pass
+
+ raise PoseEstimationFailed('No marker axis')
+
+ # Otherwise, check markers consistency
+ consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.check_markers_consistency(scene_markers, self.angle_tolerance, self.distance_tolerance)
+
+ # Pose estimation fails when no marker passes consistency checking
+ if len(consistent_markers) == 0:
+
+ raise PoseEstimationFailed('Unconsistent marker poses', unconsistencies)
+
+ # Otherwise, estimate scene pose from all consistent markers pose
+ tvec, rmat = self.aruco_scene.estimate_pose_from_markers(consistent_markers)
+
+ return tvec, rmat, 'estimate_pose_from_markers', consistent_markers
+
+ def project(self, tvec: numpy.array, rvec: numpy.array, visual_hfov: float = 0.) -> ArFrame:
+ """Project AOI scene according estimated pose and optional horizontal field of view clipping angle.
+
+ Parameters:
+ tvec: translation vector
+ rvec: rotation vector
+ visual_hfov: horizontal field of view clipping angle
+ """
+
+ # Clip AOI out of the visual horizontal field of view (optional)
+ if visual_hfov > 0:
+
+ # Transform scene into camera referential
+ aoi_3d_scene_camera_ref = self.aoi_3d_scene.transform(tvec, rvec)
+
+ # Get aoi inside vision cone field
+ cone_vision_height_cm = 200 # cm
+ cone_vision_radius_cm = numpy.tan(numpy.deg2rad(visual_hfov / 2)) * cone_vision_height_cm
+
+ _, aoi_outside = aoi_3d_scene_camera_ref.vision_cone(cone_vision_radius_cm, cone_vision_height_cm)
+
+ # Keep only aoi inside vision cone field
+ aoi_3d_scene_copy = self.aoi_3d_scene.copy(exclude=aoi_outside.keys())
+
+ else:
+
+ aoi_3d_scene_copy = self.aoi_3d_scene.copy()
+
+ # Lock camera frame exploitation
+ self.__camera_frame_lock.acquire()
+
+ # Update camera frame
+ self.camera_frame.aoi_2d_scene = aoi_3d_scene_copy.project(tvec, rvec, self._environment.aruco_detector.optic_parameters.K)
+
+ # Unlock camera frame exploitation
+ self.__camera_frame_lock.release()
+
+ # Warn user when the projected scene is empty
+ if len(self.camera_frame.aoi_2d_scene) == 0:
+
+ raise SceneProjectionFailed('AOI projection is empty')
+
+ def build_aruco_aoi_scene(self, detected_markers) -> AOI2DScene.AOI2DScene:
+ """
+ Build AOI scene from detected ArUco markers as defined in aruco_aoi dictionary.
+
+ Returns:
+ built AOI 2D scene
+ """
+
+ # Check aruco aoi is defined
+ if len(self.aruco_aoi) == 0:
+
+ raise SceneProjectionFailed('No aruco aoi is defined')
+
+ # AOI projection fails when no marker is detected
+ if len(detected_markers) == 0:
+
+ raise SceneProjectionFailed('No marker detected')
+
+ aruco_aoi_scene = {}
+
+ for aruco_aoi_name, aoi in self.aruco_aoi.items():
+
+ # Each aoi's corner is defined by a marker's corner
+ aoi_corners = []
+ for corner in ["upper_left_corner", "upper_right_corner", "lower_right_corner", "lower_left_corner"]:
+
+ marker_identifier = aoi[corner]["marker_identifier"]
+
+ try:
+
+ aoi_corners.append(detected_markers[marker_identifier].corners[0][aoi[corner]["marker_corner_index"]])
+
+ except Exception as e:
+
+ raise SceneProjectionFailed(f'Missing marker #{e} to build ArUco AOI scene')
+
+ aruco_aoi_scene[aruco_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners)
+
+ # Then each inner aoi is projected from the current aruco aoi
+ for inner_aoi_name, inner_aoi in self.aoi_3d_scene.items():
+
+ if aruco_aoi_name != inner_aoi_name:
+
+ aoi_corners = [numpy.array(aruco_aoi_scene[aruco_aoi_name].outter_axis(inner)) for inner in self.__orthogonal_projection_cache[inner_aoi_name]]
+ aruco_aoi_scene[inner_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners)
+
+ # Lock camera frame exploitation
+ self.__camera_frame_lock.acquire()
+
+ # Update camera frame
+ self.camera_frame.aoi_2d_scene = AOI2DScene.AOI2DScene(aruco_aoi_scene)
+
+ # Unlock camera frame exploitation
+ self.__camera_frame_lock.release()
+
+ def look(self, timestamp: int|float, gaze_position: GazeFeatures.GazePosition):
+ """Project timestamped gaze position into camera frame."""
+
+ # Can't use camera frame when it is locked
+ if self.__camera_frame_lock.locked():
+
+ #TODO: Store ignored timestamped gaze positions for further projections
+ print('Ignoring ', timestamp, gaze_position)
+ return
+
+ # Lock camera frame exploitation
+ self.__camera_frame_lock.acquire()
+
+ # Project gaze position in camera frame
+ yield frame.look(timestamp, inner_gaze_position * frame.size)
+
+ # Project gaze position into each aoi frames if possible
+ for aoi_name, frame in self.aoi_frames.items():
+
+ # Is aoi frame projected into camera frame ?
+ try:
+
+ aoi_2d = self.camera_frame.aoi_2d_scene[frame.name]
+
+ # TODO: Add option to use gaze precision circle
+ if aoi_2d.contains_point(gaze_position.value):
+
+ inner_x, inner_y = aoi_2d.clockwise().inner_axis(gaze_position.value)
+
+ # QUESTION: How to project gaze precision?
+ inner_gaze_position = GazeFeatures.GazePosition((inner_x, inner_y))
+
+ yield frame.look(timestamp, inner_gaze_position * frame.size)
+
+ # Ignore missing aoi frame projection
+ except KeyError:
+
+ pass
+
+ # Unlock camera frame exploitation
+ self.__camera_frame_lock.release()
+
+ def draw(self, image: numpy.array):
+ """
+ Draw camera frame
+
+ Parameters:
+ image: where to draw
+ """
+
+ self.camera_frame.aoi_2d_scene.draw(image)
+
+ def draw_axis(self, image: numpy.array):
+ """
+ Draw scene axis into image.
+
+ Parameters:
+ image: where to draw
+ """
+
+ self.aruco_scene.draw_axis(image, self._environment.aruco_detector.optic_parameters.K, self._environment.aruco_detector.optic_parameters.D)
+
+ def draw_places(self, image: numpy.array):
+ """
+ Draw scene places into image.
+
+ Parameters:
+ image: where to draw
+ """
+
+ self.aruco_scene.draw_places(image, self._environment.aruco_detector.optic_parameters.K, self._environment.aruco_detector.optic_parameters.D)