Adding aruco_aoi feature to define AOI scene from aruco marker corners.

1 files changed, 39 insertions, 9 deletions

diff --git a/src/argaze/ArScene.py b/src/argaze/ArScene.py
index 3702c6b..c8500fc 100644
--- a/src/argaze/ArScene.py
+++ b/src/argaze/ArScene.py
@@ -66,6 +66,9 @@ class ArScene():
 	aruco_axis: dict
 	"""Dictionary of orthogonal axis where each axis is defined by list of 3 markers identifier (first is origin)."""
 
+	aruco_aoi: dict
+	"""Dictionary of AOI defined by list of markers identifier and markers corners index tuples."""
+
 	def __init__(self, **kwargs):
 
 		self.aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(kwargs.pop('aruco_dictionary'))
@@ -99,20 +102,16 @@ class ArScene():
 		else:
 			self.aoi_scene = AOI3DScene.AOI3DScene(aoi_scene_value)
 
-		# TODO : allow to define AOI using marker corner position
-		'''
-    	"aruco_aoi": {
-        	"Screen": [[4, 3], [5, 2], [6, 1], [7, 0]] # [[MARKER_ID, MARKER_CORNER_ID], ...]
-    	}
-		'''
-
-		# Init axis markers
+		# Init aruco axis
 		self.aruco_axis = {}
 
+		# Init aruco aoi
+		self.aruco_aoi = {}
+
 		# Update all attributes from arguments
 		self.__dict__.update(kwargs)
 
-		# Convert axis markers identifier into ARUCO_DICT_NAME#ID string
+		# Convert aruco axis markers identifier into ARUCO_DICT_NAME#ID string
 		aruco_axis_string = {}
 		for axis_name, markers_id in self.aruco_axis.items():
 
@@ -244,6 +243,36 @@ class ArScene():
 
 		return aoi_scene_projection
 
+	def project_aruco_aoi(self, frame) -> AOI2DSceneType:
+		"""Edit AOI scene from ArUco markers into frame as defined in aruco_aoi dictionary."""
+
+		self.aruco_tracker.track(frame, estimate_pose=False)
+
+		# AOI projection fails when no marker is detected
+		if len(self.aruco_tracker.tracked_markers) == 0:
+			
+			raise SceneProjectionFailed('No marker detected')
+
+		scene_markers, _ = self.aruco_scene.filter_markers(self.aruco_tracker.tracked_markers)
+
+		# AOI projection fails when no marker belongs to the scene
+		if len(scene_markers) == 0:
+
+			raise SceneProjectionFailed('No marker belongs to the scene')
+
+		aoi_scene = {}
+
+		for name, marker_corners in self.aruco_aoi.items():
+
+			aoi_points = []
+			for marker_id, corner_id in marker_corners:
+
+				aoi_points.append(self.aruco_tracker.tracked_markers[marker_id].corners[0][corner_id])
+
+			aoi_scene[name] = AOIFeatures.AreaOfInterest(aoi_points)
+
+		return AOI2DScene.AOI2DScene(aoi_scene)
+
 	def draw_axis(self, frame):
 		"""Draw scene axis into frame."""
 
@@ -253,4 +282,5 @@ class ArScene():
 		"""Draw scene places into frame."""
 
 		self.aruco_scene.draw_places(frame, self.aruco_camera.K, self.aruco_camera.D)
+