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
|
"""ArScene based of ArUco markers technology.
This program is free software: you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation, either version 3 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with
this program. If not, see <http://www.gnu.org/licenses/>.
"""
__author__ = "Théo de la Hogue"
__credits__ = []
__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)"
__license__ = "GPLv3"
import numpy
from argaze import ArFeatures, DataFeatures
from argaze.ArUcoMarkers import ArUcoMarkersGroup
class ArUcoScene(ArFeatures.ArScene):
"""
Define an ArScene based on an ArUcoMarkersGroup description.
"""
@DataFeatures.PipelineStepInit
def __init__(self, **kwargs):
"""Initialize ArUcoScene"""
# Init ArScene classes
super().__init__()
# Init private attribute
self.__aruco_markers_group = None
@property
def aruco_markers_group(self) -> ArUcoMarkersGroup.ArUcoMarkersGroup:
"""ArUco markers 3D scene description used to estimate scene pose from detected markers: see [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function below."""
return self.__aruco_markers_group
@aruco_markers_group.setter
@DataFeatures.PipelineStepAttributeSetter
def aruco_markers_group(self, aruco_markers_group: ArUcoMarkersGroup.ArUcoMarkersGroup):
self.__aruco_markers_group = aruco_markers_group
# Edit parent
if self.__aruco_markers_group is not None:
self.__aruco_markers_group.parent = self
@DataFeatures.PipelineStepMethod
def estimate_pose(self, detected_markers: dict) -> tuple[numpy.array, numpy.array, dict]:
"""Estimate scene pose from detected ArUco markers.
Parameters:
detected_markers: dictionary with all detected markers
Returns:
scene translation vector
scene rotation matrix
dict of markers used to estimate the pose
"""
# Pose estimation fails when no marker is detected
if len(detected_markers) == 0:
raise ArFeatures.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 ArFeatures.PoseEstimationFailed('No marker belongs to the scene')
# Pose estimation fails if only one marker belongs to the scene
if len(scene_markers) == 1:
raise ArFeatures.PoseEstimationFailed('Only one marker belongs to the scene')
# Estimate pose from a markers corners
success, tvec, rmat = self.__aruco_markers_group.estimate_pose_from_markers_corners(scene_markers, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D)
if not success:
raise ArFeatures.PoseEstimationFailed('Can\'t estimate pose from markers corners positions')
return tvec, rmat, scene_markers
def draw(self, image: numpy.array, draw_aruco_markers_group: dict = None):
"""
Draw scene into image.
Parameters:
image: where to draw
draw_aruco_markers_group: ArUcoMarkersGroup.draw parameters (if None, no group drawn)
"""
# Draw group if required
if draw_aruco_markers_group is not None:
self.__aruco_markers_group.draw(image, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D, **draw_aruco_markers_group)
|
