Working around aruco_aoi inner aoi projections.

1 files changed, 22 insertions, 18 deletions

diff --git a/src/argaze/ArScene.py b/src/argaze/ArScene.py
index 35fef1c..630ee62 100644
--- a/src/argaze/ArScene.py
+++ b/src/argaze/ArScene.py
@@ -124,6 +124,13 @@ class ArScene():
 
 		self.aruco_axis = aruco_axis_string
 
+		# Preprocess a default whole scene projection to speed up further aruco aoi processings
+		_, tvec, rvec, K = self.whole_pose()
+		self.__default_whole_scene_projection = self.aoi_scene.project(tvec, rvec, K)
+
+		# DEBUG
+		print('self.__default_whole_scene_projection:\n', self.__default_whole_scene_projection )
+
 	@classmethod
 	def from_json(self, json_filepath: str) -> ArSceneType:
 		"""Load ArGaze project from .json file."""
@@ -147,16 +154,15 @@ class ArScene():
 
 		return output
 
-	def whole_pose(self, width: float = 1., height: float = None) -> Tuple[numpy.array, numpy.array, numpy.array, numpy.array]:
+	def whole_pose(self, width: float = 1., height: float = 0.) -> Tuple[numpy.array, numpy.array, numpy.array, numpy.array]:
 		"""Edit translation vector, rotation vector and camera intrinsic parameters to setup a whole scene projection frame.
 
 		* **Arguments:** 
 			- frame width
 			- frame height: optional, if None the height will be setup according given width and scene dimensions ratio.
 		"""
-
 		scene_size = self.aoi_scene.size
-		frame_size = numpy.array([width, int(scene_size[1]/scene_size[0]*width) if height == None else height])
+		frame_size = numpy.array([width, scene_size[1]/scene_size[0]*width if height == 0. else height])
 		tvec = self.aoi_scene.center*[-1, 1, 0] + [0, 0, scene_size[1]]
 		rvec = numpy.array([[-numpy.pi, 0.0, 0.0]])
 		K = numpy.array([[frame_size[1], 0.0, frame_size[0]/2], [0.0, frame_size[1], frame_size[1]/2], [0.0, 0.0, 1.0]])
@@ -280,29 +286,27 @@ class ArScene():
 
 			raise SceneProjectionFailed('No marker belongs to the scene')
 
-		aoi_scene = {}
-
-		for name, data in self.aruco_aoi.items():
-
-			if type(data) == list:
+		aruco_aoi_scene = {}
 
-				marker_corners = data
+		for aruco_aoi_name, aoi in self.aruco_aoi.items():
 
-				aoi_points = []
-				for marker_id, corner_id in marker_corners:
+			# Each aoi's corner is defined by a marker's corner
+			aoi_corners = []
+			for corner in ["top_left_corner", "top_right_corner", "bottom_left_corner", "bottom_right_corner"]:
 
-					aoi_points.append(self.aruco_tracker.tracked_markers[marker_id].corners[0][corner_id])
+				aoi_corners.append(self.aruco_tracker.tracked_markers[aoi[corner]["marker_identifier"]].corners[0][aoi[corner]["marker_corner_index"]])
 
-				aoi_scene[name] = AOIFeatures.AreaOfInterest(aoi_points)
+			aruco_aoi_scene[aruco_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners)
 
-			elif type(data) == dict:
+			# Then each inner aoi is projected from the current aruco aoi
+			for inner_aoi_name, inner_aoi in self.aoi_scene.items():
 
-				parent_aoi = aoi_scene[data['parent']]
-				aoi_points = [numpy.array(parent_aoi.outter_axis(inner)) for inner in data['inner_points']]
+				if aruco_aoi_name != inner_aoi_name:
 
-				aoi_scene[name] = AOIFeatures.AreaOfInterest(aoi_points)
+					aoi_corners = [numpy.array(aruco_aoi_scene[aruco_aoi_name].outter_axis(inner)) for inner in self.__default_whole_scene_projection[inner_aoi_name]]
+					aruco_aoi_scene[inner_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners)
 
-		return AOI2DScene.AOI2DScene(aoi_scene)
+		return AOI2DScene.AOI2DScene(aruco_aoi_scene)
 
 	def draw_axis(self, frame):
 		"""Draw scene axis into frame."""