1 files changed, 82 insertions, 0 deletions
diff --git a/src/argaze/utils/estimate_markers_pose/observers.py b/src/argaze/utils/estimate_markers_pose/observers.py
new file mode 100644
index 0000000..23316ca
--- /dev/null
+++ b/src/argaze/utils/estimate_markers_pose/observers.py
@@ -0,0 +1,82 @@
+""" """
+
+"""
+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 <https://www.gnu.org/licenses/>.
+"""
+
+__author__ = "Théo de la Hogue"
+__credits__ = []
+__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)"
+__license__ = "GPLv3"
+
+import logging
+
+from argaze import DataFeatures
+from argaze.ArUcoMarker import ArUcoMarkerGroup
+
+
+class ArUcoMarkersPoseRecorder(DataFeatures.PipelineStepObject):
+
+	@DataFeatures.PipelineStepInit
+	def __init__(self, **kwargs):
+
+		# Init private attributes
+		self.__output_folder = None
+		self.__size = None
+		self.__ids = None
+
+	@property
+	def output_folder(self) -> str:
+		"""folder path where to write ArUco markers pose."""
+		return self.__output_folder
+
+	@output_folder.setter
+	def output_folder(self, output_folder: str):
+
+		self.__output_folder = output_folder
+
+	@property
+	def size(self) -> float:
+		"""Expected size in centimeters of detected markers."""
+		return self.__output_folder
+
+	@size.setter
+	def size(self, size: float):
+
+		self.__size = size
+
+	@property
+	def ids(self) -> list:
+		"""Ids of markers to estimate pose (default all)."""
+		return self.__ids
+
+	@ids.setter
+	def ids(self, ids: list):
+
+		self.__ids = ids
+
+	def on_detect_markers(self, timestamp, aruco_detector, exception):
+
+		logging.info('%s writes estimated markers pose into %s', DataFeatures.get_class_path(self), self.__output_folder)
+
+		if self.__size is not None:
+
+			# Estimate all detected markers pose
+			aruco_detector.estimate_markers_pose(self.__size, ids = self.__ids)
+
+			# Build ArUco markers group from detected markers
+			aruco_markers_group = ArUcoMarkerGroup.ArUcoMarkerGroup(dictionary=aruco_detector.dictionary, places=aruco_detector.detected_markers())
+
+		if self.__output_folder is not None:
+
+			# Write ArUco markers group
+			aruco_markers_group.to_obj(f'{self.__output_folder}/{int(timestamp)}-aruco_markers_group.obj')
+			
+\ No newline at end of file