1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
|
#!/usr/bin/env python
from typing import TypeVar
from dataclasses import dataclass, field
import json
import os
from argaze.ArUcoMarkers import *
from argaze.AreaOfInterest import *
import numpy
ArSceneType = TypeVar('ArScene', bound="ArScene")
# Type definition for type annotation convenience
class PoseEstimationFailed(Exception):
"""Exception raised by ArScene project method when the pose can't be estimated due to unconsistencies."""
def __init__(self, message, unconsistencies=None):
super().__init__(message)
self.unconsistencies = unconsistencies
class SceneProjectionFailed(Exception):
"""Exception raised by ArScene project method when the scene can't be projected."""
def __init__(self, message):
super().__init__(message)
@dataclass
class ArScene():
"""Define an Augmented Reality environnement thanks to ArUco markers and project it onto incoming frames."""
name: str
"""Project name."""
aruco_dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(init=False, default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary)
"""ArUco markers dictionary."""
aruco_marker_size: float = field(init=False)
"""Size of ArUco markers in centimeter."""
aruco_camera: ArUcoCamera.ArUcoCamera = field(init=False, default_factory=ArUcoCamera.ArUcoCamera)
"""ArUco camera ..."""
aruco_tracker: ArUcoTracker.ArUcoTracker = field(init=False, default_factory=ArUcoTracker.ArUcoTracker)
"""ArUco tracker ..."""
aruco_scene: ArUcoScene.ArUcoScene = field(init=False, default_factory=ArUcoScene.ArUcoScene)
"""ArUco scene ..."""
aoi_scene: AOI3DScene.AOI3DScene = field(init=False, default_factory=AOI3DScene.AOI3DScene)
"""AOI 3D scene ..."""
def __init__(self, **kwargs):
self.aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(kwargs.pop('aruco_dictionary'))
self.aruco_marker_size = kwargs.pop('aruco_marker_size')
self.aruco_camera = ArUcoCamera.ArUcoCamera(**kwargs.pop('aruco_camera'))
self.aruco_tracker = ArUcoTracker.ArUcoTracker(self.aruco_dictionary, self.aruco_marker_size, self.aruco_camera, **kwargs.pop('aruco_tracker'))
# Check aruco_scene.places value type
aruco_scene_places_value = kwargs['aruco_scene']['places']
# str: relative path to .obj file
if type(aruco_scene_places_value) == str:
kwargs['aruco_scene']['places'] = os.path.join(self.__current_directory, aruco_scene_places_value)
self.aruco_scene = ArUcoScene.ArUcoScene(self.aruco_dictionary, self.aruco_marker_size, **kwargs.pop('aruco_scene'))
# Check aoi_scene value type
aoi_scene_value = kwargs.pop('aoi_scene')
# str: relative path to .obj file
if type(aoi_scene_value) == str:
obj_filepath = os.path.join(self.__current_directory, aoi_scene_value)
self.aoi_scene = AOI3DScene.AOI3DScene.from_obj(obj_filepath)
# dict: all AOI
else:
self.aoi_scene = AOI3DScene.AOI3DScene(aoi_scene_value)
self.__dict__.update(kwargs)
@classmethod
def from_json(self, json_filepath: str) -> ArSceneType:
"""Load ArGaze project from .json file."""
with open(json_filepath) as configuration_file:
# Store current directory to allow relative path loading
self.__current_directory = os.path.dirname(os.path.abspath(json_filepath))
return ArScene(**json.load(configuration_file))
def __str__(self) -> str:
"""String display"""
output = ''
output += f'\nArUcoCamera: {self.aruco_camera}'
output += f'\n\nArUcoTracker tracking data: {self.aruco_tracker.tracking_data}'
output += f'\n\nArUcoScene: {self.aruco_scene}'
output += f'\n\nAOIScene: {self.aoi_scene}'
output += '\n'
return output
def project(self, frame, consistent_markers_number:int = 1, visual_hfov=0, pre_tracked_markers=False):
"""Project ArScene into frame."""
# Track markers with pose estimation if it not already done
if not pre_tracked_markers:
self.aruco_tracker.track(frame)
# When no marker is detected, no AOI scene projection can't be done
if len(self.aruco_tracker.tracked_markers) == 0:
raise PoseEstimationFailed('No marker detected')
# Estimate set pose from tracked markers
tvec, rvec, success, consistent_markers, unconsistencies = self.aruco_scene.estimate_pose(self.aruco_tracker.tracked_markers)
# When pose estimation fails, ignore AOI scene projection
if not success:
raise PoseEstimationFailed('Unconsistent marker poses', unconsistencies)
# Consider pose estimation only if theer is a given number of consistent markers at least
elif len(consistent_markers) >= consistent_markers_number:
# Clip AOI out of the visual horizontal field of view (optional)
if visual_hfov > 0:
# Transform scene into camera referential
aoi_scene_camera_ref = self.aoi_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_scene_camera_ref.vision_cone(cone_vision_radius_cm, cone_vision_height_cm)
# Keep only aoi inside vision cone field
aoi_scene_copy = self.aoi_scene.copy(exclude=aoi_outside.keys())
else:
aoi_scene_copy = self.aoi_scene.copy()
# 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.
aoi_scene_projection = aoi_scene_copy.project(tvec, rvec, self.aruco_camera.K)
# Warn user when the projected scene is empty
if len(aoi_scene_projection) == 0:
raise SceneProjectionFailed('AOI projection is empty')
return aoi_scene_projection
|
