Moving pose estimation after marker detection if expected size is given.

4 files changed, 52 insertions, 261 deletions

diff --git a/src/argaze/ArUcoMarker/ArUcoDetector.py b/src/argaze/ArUcoMarker/ArUcoDetector.py
index 32a7f3f..32091a4 100644
--- a/src/argaze/ArUcoMarker/ArUcoDetector.py
+++ b/src/argaze/ArUcoMarker/ArUcoDetector.py
@@ -26,7 +26,7 @@ import numpy
 from cv2 import aruco
 
 from argaze import DataFeatures
-from argaze.ArUcoMarker import ArUcoMarkerDictionary, ArUcoMarker, ArUcoOpticCalibrator
+from argaze.ArUcoMarker import ArUcoMarkerDictionary, ArUcoMarker, ArUcoOpticCalibrator, ArUcoMarkerGroup
 
 
 class DetectorParameters():
@@ -139,6 +139,11 @@ class ArUcoDetector(DataFeatures.PipelineStepObject):
 		# Init detected markers data
 		self.__detected_markers = {}
 
+		# Init pose estimation data
+		self.__pose_size = None
+		self.__pose_ids = []
+		self.__pose = None
+
 		# Init detected board data
 		self.__board = None
 		self.__board_corners_number = 0
@@ -187,6 +192,9 @@ class ArUcoDetector(DataFeatures.PipelineStepObject):
 
 		!!! danger "DON'T UNDISTORTED IMAGE"
 			Camera intrinsic parameters and distortion coefficients are used later during pose estimation.
+		
+		!!! note
+			The pose of markers will be also estimated if the pose_size attribute is not None.
 		"""
 
 		# Reset detected markers data
@@ -202,6 +210,7 @@ class ArUcoDetector(DataFeatures.PipelineStepObject):
 			detected_markers_ids = detected_markers_ids.T[0]
 
 			for i, marker_id in enumerate(detected_markers_ids):
+
 				marker = ArUcoMarker.ArUcoMarker(self.__dictionary, marker_id)
 				marker.corners = detected_markers_corners[i][0]
 
@@ -212,12 +221,48 @@ class ArUcoDetector(DataFeatures.PipelineStepObject):
 
 				self.__detected_markers[marker_id] = marker
 
+			# Estimate markers pose if required
+			if self.__pose_size is not None:
+
+				self.estimate_markers_pose(self.__pose_size, self.__pose_ids)
+
+				self.__pose = ArUcoMarkerGroup.ArUcoMarkerGroup(dictionary=self.__dictionary, places=self.__detected_markers)
+
+	@property
+	def pose_size(self) -> float:
+		"""Expected markers size in centimeters to enable pose estimation."""
+		return self.__pose_size
+
+	@pose_size.setter
+	@DataFeatures.PipelineStepAttributeSetter
+	def pose_size(self, pose_size: float):
+
+		self.__pose_size = pose_size
+
+	@property
+	def pose_ids(self) -> list:
+		"""Ids of markers to select for pose estimation (default all)."""
+		return self.__pose_ids
+
+	@pose_ids.setter
+	def pose_ids(self, pose_ids: list):
+
+		self.__pose_ids = pose_ids
+
+	@property
+	def pose(self) -> ArUcoMarkerGroup:
+		"""The estimated pose of detected markers considering their expected size."""
+		return self.__pose
+
 	def estimate_markers_pose(self, size: float, ids: list = []):
-		"""Estimate pose detected markers pose considering a marker size.
+		"""Estimate detected markers pose considering a marker size.
 
 		Parameters:
 			size: size of markers in centimeters.
 			ids: markers id list to select detected markers.
+
+		!!! warning
+			This method have to called after 'detect_markers'
 		"""
 
 		# Is there detected markers ?
diff --git a/src/argaze/utils/aruco_marker_group_export.py b/src/argaze/utils/aruco_marker_group_export.py
deleted file mode 100644
index 47aba86..0000000
--- a/src/argaze/utils/aruco_marker_group_export.py
+++ /dev/null
@@ -1,226 +0,0 @@
-#!/usr/bin/env python
-
-""" """
-
-"""
-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 argparse
-import contextlib
-
-import cv2
-
-from argaze import DataFeatures
-from argaze.ArUcoMarker import ArUcoDetector, ArUcoOpticCalibrator, ArUcoMarkerGroup
-from argaze.utils import UtilsFeatures
-
-
-def main():
-	"""
-	Detect DICTIONARY and SIZE ArUco markers inside a MOVIE frame then, export detected ArUco markers group as .obj file into an OUTPUT folder.
-	"""
-
-	# Manage arguments
-	parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
-	parser.add_argument('movie', metavar='MOVIE', type=str, default=None, help='movie path')
-	parser.add_argument('dictionary', metavar='DICTIONARY', type=str, default=None, help='expected ArUco markers dictionary')
-	parser.add_argument('size', metavar='SIZE', type=float, default=None, help='expected ArUco markers size (in cm)')
-
-	parser.add_argument('-p', '--parameters', metavar='PARAMETERS', type=str, default=None, help='ArUco detector parameters file')
-	parser.add_argument('-op', '--optic_parameters', metavar='OPTIC_PARAMETERS', type=str, default=None, help='ArUco detector optic parameters file')
-
-	parser.add_argument('-s', '--start', metavar='START', type=float, default=0., help='start time in second')
-	parser.add_argument('-o', '--output', metavar='OUTPUT', type=str, default='.', help='export folder path')
-	parser.add_argument('-v', '--verbose', action='store_true', default=False,
-						help='enable verbose mode to print information in console')
-
-	args = parser.parse_args()
-
-	# Load movie
-	video_capture = cv2.VideoCapture(args.movie)
-
-	video_fps = video_capture.get(cv2.CAP_PROP_FPS)
-	image_width = int(video_capture.get(cv2.CAP_PROP_FRAME_WIDTH))
-	image_height = int(video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
-
-	# Edit ArUco detector configuration
-	configuration = {
-		"dictionary": args.dictionary
-	}
-
-	if args.parameters:
-		configuration["parameters"] = args.parameters
-
-	if args.optic_parameters:
-		configuration["optic_parameters"] = args.optic_parameters
-
-	# Load ArUco detector configuration
-	aruco_detector = DataFeatures.from_dict(ArUcoDetector.ArUcoDetector, configuration)
-
-	# Create default optic parameters adapted to frame size
-	if aruco_detector.optic_parameters is None:
-
-		# Note: The choice of 1000 for default focal length should be discussed...
-		aruco_detector.optic_parameters = ArUcoOpticCalibrator.OpticParameters(rms=-1, dimensions=(image_width, image_height), K=ArUcoOpticCalibrator.K0(focal_length=(1000., 1000.), width=image_width, height=image_height))
-
-	if args.verbose:
-		print(aruco_detector)
-
-	# Create empty ArUco scene
-	aruco_markers_group = None
-
-	# Edit draw parameters
-	draw_parameters = {
-		"color": [255, 255, 255],
-		"draw_axes": {
-			"thickness": 4
-		}
-	}
-
-	# Create a window
-	cv2.namedWindow("Export detected ArUco markers", cv2.WINDOW_AUTOSIZE)
-
-	# Init image selection
-	current_image_index = -1
-	_, current_image = video_capture.read()
-	next_image_index = int(args.start * video_fps)
-	refresh = False
-
-	# Waiting for 'ctrl+C' interruption
-	with contextlib.suppress(KeyboardInterrupt):
-
-		while True:
-
-			# Select a new image and detect markers once
-			if next_image_index != current_image_index or refresh:
-
-				video_capture.set(cv2.CAP_PROP_POS_FRAMES, next_image_index)
-
-				success, video_image = video_capture.read()
-
-				video_height, video_width, _ = video_image.shape
-
-				if success:
-
-					# Refresh once
-					refresh = False
-
-					current_image_index = video_capture.get(cv2.CAP_PROP_POS_FRAMES) - 1
-					current_image_time = video_capture.get(cv2.CAP_PROP_POS_MSEC)
-
-					# Timestamp image
-					video_image = DataFeatures.TimestampedImage(video_image, timestamp=current_image_time)
-
-					try:
-
-						# Detect and project AR features
-						aruco_detector.detect_markers(video_image)
-
-						# Estimate all detected markers pose
-						aruco_detector.estimate_markers_pose(args.size)
-
-						# Build aruco scene from detected markers
-						aruco_markers_group = ArUcoMarkerGroup.ArUcoMarkerGroup(dictionary=aruco_detector.dictionary, places=aruco_detector.detected_markers())
-
-						# Detection succeeded
-						exception = None
-
-					# Write errors
-					except Exception as e:
-
-						aruco_markers_group = None
-
-						exception = e
-
-					# Draw detected markers
-					aruco_detector.draw_detected_markers(video_image, draw_parameters)
-
-					# Write detected markers
-					cv2.putText(video_image, f'Detecting markers {list(aruco_detector.detected_markers().keys())}', (20, video_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
-					# Write timing
-					cv2.putText(video_image, f'Frame at {int(current_image_time)}ms', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
-					# Write exception
-					if exception is not None:
-						cv2.putText(video_image, f'error: {exception}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-
-					# Write documentation
-					cv2.putText(video_image, f'<- previous image', (video_width - 500, video_height - 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-					cv2.putText(video_image, f'-> next image', (video_width - 500, video_height - 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-					cv2.putText(video_image, f'r: reload config', (video_width - 500, video_height - 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-					cv2.putText(video_image, f'Ctrl+s: export ArUco markers', (video_width - 500, video_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-
-					# Copy image
-					current_image = video_image.copy()
-
-			# Keep last image
-			else:
-
-				video_image = current_image.copy()
-
-			key_pressed = cv2.waitKey(10)
-
-			#if key_pressed != -1:
-			#	print(key_pressed)
-
-			# Select previous image with left arrow
-			if key_pressed == 2:
-				next_image_index -= 1
-
-			# Select next image with right arrow
-			if key_pressed == 3:
-				next_image_index += 1
-
-			# Clip image index
-			if next_image_index < 0:
-				next_image_index = 0
-
-			# r: reload configuration
-			if key_pressed == 114:
-				aruco_detector = DataFeatures.from_dict(ArUcoDetector.ArUcoDetector, configuration)
-				refresh = True
-				print('Configuration reloaded')
-
-			# Save selected marker edition using 'Ctrl + s'
-			if key_pressed == 19:
-
-				if aruco_markers_group:
-
-					aruco_markers_group.to_obj(f'{args.output}/{int(current_image_time)}-aruco_markers_group.obj')
-					print(f'ArUco markers saved into {args.output}')
-
-				else:
-
-					print(f'No ArUco markers to export')
-
-			# Close window using 'Esc' key
-			if key_pressed == 27:
-				break
-
-			# Display video
-			cv2.imshow(aruco_detector.name, video_image)
-
-	# Close movie capture
-	video_capture.release()
-
-	# Stop image display
-	cv2.destroyAllWindows()
-
-
-if __name__ == '__main__':
-	main()
diff --git a/src/argaze/utils/estimate_markers_pose/observers.py b/src/argaze/utils/estimate_markers_pose/observers.py
index 23316ca..88da4f9 100644
--- a/src/argaze/utils/estimate_markers_pose/observers.py
+++ b/src/argaze/utils/estimate_markers_pose/observers.py
@@ -43,40 +43,12 @@ class ArUcoMarkersPoseRecorder(DataFeatures.PipelineStepObject):
 
 		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:
+		if aruco_detector.pose is not None:
 
 			# Write ArUco markers group
-			aruco_markers_group.to_obj(f'{self.__output_folder}/{int(timestamp)}-aruco_markers_group.obj')
+			aruco_detector.pose.to_obj(f'{self.__output_folder}/{int(timestamp)}-aruco_markers_group.obj')
 			
\ No newline at end of file
diff --git a/src/argaze/utils/estimate_markers_pose/pipeline.json b/src/argaze/utils/estimate_markers_pose/pipeline.json
index c3034e8..2e0ab76 100644
--- a/src/argaze/utils/estimate_markers_pose/pipeline.json
+++ b/src/argaze/utils/estimate_markers_pose/pipeline.json
@@ -4,14 +4,14 @@
         "size": [1920, 1080],
         "aruco_detector": {
             "dictionary": "DICT_APRILTAG_16h5",
+            "pose_size": 4,
+            "pose_ids": [],
             "parameters": {
                 "useAruco3Detection": 1
             },
             "observers":{
                 "observers.ArUcoMarkersPoseRecorder": {
-                    "output_folder": "_export/records/aruco_markers_group",
-                    "size": 4,
-                    "ids": []
+                    "output_folder": "_export/records/aruco_markers_group"
                 }
             }
         },