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
|
#!/usr/bin/env python
"""ArScene based of ArUco markers technology."""
__author__ = "Théo de la Hogue"
__credits__ = []
__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)"
__license__ = "BSD"
from typing import TypeVar, Tuple
from dataclasses import dataclass, field
import json
import os
from argaze import ArFeatures, DataStructures
from argaze.ArUcoMarkers import ArUcoMarkersGroup
from argaze.AreaOfInterest import AOI2DScene
import cv2
import numpy
ArUcoSceneType = TypeVar('ArUcoScene', bound="ArUcoScene")
# Type definition for type annotation convenience
@dataclass
class ArUcoScene(ArFeatures.ArScene):
"""
Define an ArScene based on an ArUcoMarkersGroup description.
Parameters:
aruco_markers_group: ArUco markers 3D scene description used to estimate scene pose from detected markers: see [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function below.
"""
aruco_markers_group: ArUcoMarkersGroup.ArUcoMarkersGroup = field(default_factory=ArUcoMarkersGroup.ArUcoMarkersGroup)
def __post_init__(self):
super().__post_init__()
def __str__(self) -> str:
"""
Returns:
String representation
"""
output = output = super().__str__()
output += f'ArUcoMarkersGroup:\n{self.aruco_markers_group}\n'
return output
@classmethod
def from_dict(self, aruco_scene_data: dict, working_directory: str = None) -> ArUcoSceneType:
"""
Load ArUcoScene from dictionary.
Parameters:
aruco_scene_data: dictionary
working_directory: folder path where to load files when a dictionary value is a relative filepath.
"""
# Load aruco markers group
try:
# Check aruco_markers_group value type
aruco_markers_group_value = aruco_scene_data.pop('aruco_markers_group')
# str: relative path to .obj file
if type(aruco_markers_group_value) == str:
aruco_markers_group_value = os.path.join(working_directory, aruco_markers_group_value)
new_aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup.from_obj(aruco_markers_group_value)
# dict:
else:
new_aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup(**aruco_markers_group_value)
except KeyError:
new_aruco_markers_group = None
# Get values of temporary ar scene created from aruco_scene_data
temp_ar_scene_values = DataStructures.as_dict(ArFeatures.ArScene.from_dict(aruco_scene_data, working_directory))
# Create new aruco scene using temporary ar scene values
return ArUcoScene(aruco_markers_group=new_aruco_markers_group, **temp_ar_scene_values)
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_markers_group.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_markers_group.estimate_pose_from_single_marker(marker)
return tvec, rmat, 'estimate_pose_from_single_marker', {marker_id: marker}
# Otherwise, check markers consistency
consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.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_markers_group.estimate_pose_from_markers(consistent_markers)
return tvec, rmat, 'estimate_pose_from_markers', consistent_markers
def draw_axis(self, image: numpy.array):
"""
Draw scene axis into image.
Parameters:
image: where to draw
"""
self.aruco_markers_group.draw_axis(image, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D)
def draw_places(self, image: numpy.array):
"""
Draw scene places into image.
Parameters:
image: where to draw
"""
self.aruco_markers_group.draw_places(image, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D)
|
