Adding projection cache feature.

2 files changed, 199 insertions, 41 deletions

diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py
index baa26a1..a37a4d3 100644
--- a/src/argaze/ArFeatures.py
+++ b/src/argaze/ArFeatures.py
@@ -19,6 +19,7 @@ __license__ = "GPLv3"
 import logging
 import math
 import os
+import ast
 from typing import Iterator, Union
 
 import cv2
@@ -1072,6 +1073,10 @@ class ArCamera(ArFrame):
 		# Init private attributes
 		self.__visual_hfov = 0.
 		self.__visual_vfov = 0.
+		self.__projection_cache = None
+		self.__projection_cache_writer = None
+		self.__projection_cache_reader = None
+		self.__projection_cache_data = None
 
 		# Init protected attributes
 		self._scenes = {}
@@ -1133,6 +1138,128 @@ class ArCamera(ArFrame):
 		"""Set camera's visual vertical field of view."""
 		self.__visual_vfov = value
 
+	@property
+	def projection_cache(self) -> str:
+		"""file path to store/read layers projections into/from a cache."""
+		return self.__projection_cache
+
+	@projection_cache.setter
+	def projection_cache(self, projection_cache: str):
+
+		self.__projection_cache = projection_cache
+
+		# The file doesn't exist yet: store projections into the cache
+		if not os.path.exists(os.path.join( DataFeatures.get_working_directory(), self.__projection_cache) ):
+
+			self.__projection_cache_writer = UtilsFeatures.FileWriter(path=self.__projection_cache)
+			self.__projection_cache_reader = None
+
+		# The file exist: read projection from the cache
+		else:
+
+			self.__projection_cache_writer = None
+			self.__projection_cache_reader = UtilsFeatures.FileReader(path=self.__projection_cache)
+			
+	def _clear_projection(self):
+		"""Clear layers projection."""
+
+		logging.debug('ArCamera._clear_projection %s', self.name)
+
+		for layer_name, layer in self.layers.items():
+
+			# Initialize layer if needed
+			if layer.aoi_scene is None:
+
+				layer.aoi_scene = AOI2DScene.AOI2DScene()
+
+			else:
+
+				layer.aoi_scene.clear()
+
+	def _write_projection_cache(self, timestamp: int|float, exception = None):
+		"""Write layers aoi scene into the projection cache.
+
+		Parameters:
+			timestamp: cache time
+		"""
+
+		if self.__projection_cache_writer is not None:
+
+			logging.debug('ArCamera._write_projection_cache %s %f', self.name, timestamp)
+
+			if exception is None:
+
+				projection = {}
+
+				for layer_name, layer in self.layers.items():
+
+					projection[layer_name] = layer.aoi_scene
+
+				self.__projection_cache_writer.write( (timestamp, projection) )
+
+			else:
+
+				self.__projection_cache_writer.write( (timestamp, exception) )
+
+	def _read_projection_cache(self, timestamp: int|float):
+		"""Read layers aoi scene from the projection cache.
+
+		Parameters:
+			timestamp: cache time.
+
+		Returns:
+			success: False if there is no projection cache, True otherwise.
+		"""
+
+		if self.__projection_cache_reader is None:
+
+			return False
+
+		logging.debug('ArCamera._read_projection_cache %s %f', self.name, timestamp)
+
+		# Clear former projection
+		self._clear_projection()
+
+		try:
+
+			# Read first data if not done yet
+			if self.__projection_cache_data is None:
+
+				self.__projection_cache_data = self.__projection_cache_reader.read()
+
+			# Continue reading cache until correct timestamped projection
+			while float(self.__projection_cache_data[0]) < timestamp:
+
+				self.__projection_cache_data = self.__projection_cache_reader.read()
+
+		# No more projection in the cache
+		except EOFError:
+			
+			raise DataFeatures.TimestampedException("Projection cache is empty", timestamp=timestamp)
+
+		# Correct timestamped projection is found
+		if float(self.__projection_cache_data[0]) == timestamp:
+			
+			# When correct timestamped projection is found
+			projection = {}
+
+			try:
+
+				projection = ast.literal_eval(self.__projection_cache_data[1])
+
+				for layer_name, aoi_scene in projection.items():
+
+					self._layers[layer_name].aoi_scene = AOI2DScene.AOI2DScene(aoi_scene)
+					self._layers[layer_name].timestamp = timestamp
+
+					logging.debug('> reading %s projection from cache', layer_name)
+					
+			except SyntaxError as e:
+
+				raise DataFeatures.TimestampedException(self.__projection_cache_data[1], timestamp=timestamp)
+
+		return True
+
 	def scene_frames(self) -> Iterator[ArFrame]:
 		"""Iterate over all scenes frames"""
 
@@ -1153,6 +1280,28 @@ class ArCamera(ArFrame):
 			"visual_vfov": self.__visual_vfov
 		}
 
+	@DataFeatures.PipelineStepEnter
+	def __enter__(self):
+
+		if self.__projection_cache_writer is not None:
+
+			self.__projection_cache_writer.__enter__()
+
+		if self.__projection_cache_reader is not None:
+
+			self.__projection_cache_reader.__enter__()
+
+	@DataFeatures.PipelineStepExit
+	def __exit__(self, exception_type, exception_value, exception_traceback):
+
+		if self.__projection_cache_writer is not None:
+
+			self.__projection_cache_writer.__exit__(exception_type, exception_value, exception_traceback)
+		
+		if self.__projection_cache_reader is not None:
+
+			self.__projection_cache_reader.__exit__(exception_type, exception_value, exception_traceback)
+
 	def _update_expected_and_excluded_aoi(self):
 		"""Edit expected aoi of each layer aoi scan path with the aoi of corresponding scene layer.
 		Edit excluded aoi to ignore frame aoi from aoi matching.
@@ -1468,11 +1617,14 @@ class ArContext(DataFeatures.PipelineStepObject):
 
 		logging.debug('\t> get image (%i x %i)', width, height)
 
+		last_position = self.__pipeline.last_gaze_position()
+
 		info_stack = 0
 
 		if draw_times:
 
 			if image.is_timestamped():
+
 				info_stack += 1
 				cv2.putText(image, f'Frame at {image.timestamp}ms', (20, info_stack * 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
 
@@ -1489,6 +1641,11 @@ class ArContext(DataFeatures.PipelineStepObject):
 				info_stack += 1
 				cv2.putText(image, f'Watch {watch_time}ms at {self.__process_camera_image_frequency}Hz', (20, info_stack * 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
 
+			if last_position is not None:
+
+				info_stack += 1
+				cv2.putText(image, f'Position at {last_position.timestamp}ms', (20, info_stack * 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+
 			if issubclass(type(self.__pipeline), ArFrame):
 
 				try:
diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py
index 95df135..5b535b5 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCamera.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py
@@ -79,12 +79,7 @@ class ArUcoCamera(ArFeatures.ArCamera):
 
 			# Create default optic parameters adapted to frame size
 			# Note: The choice of 1000 for default focal length should be discussed...
-			self.__aruco_detector.optic_parameters = ArUcoOpticCalibrator.OpticParameters(rms=-1, dimensions=self.size,
-			                                                                              K=ArUcoOpticCalibrator.K0(
-				                                                                              focal_length=(
-				                                                                              1000., 1000.),
-				                                                                              width=self.size[0],
-				                                                                              height=self.size[1]))
+			self.__aruco_detector.optic_parameters = ArUcoOpticCalibrator.OpticParameters(rms=-1, dimensions=self.size, K=ArUcoOpticCalibrator.K0(focal_length=(1000., 1000.), width=self.size[0], height=self.size[1]))
 
 		# Edit parent
 		if self.__aruco_detector is not None:
@@ -134,64 +129,70 @@ class ArUcoCamera(ArFeatures.ArCamera):
 				cv2.rectangle(image, (0, 0), (self.__sides_mask, height), (0, 0, 0), -1)
 				cv2.rectangle(image, (width - self.__sides_mask, 0), (width, height), (0, 0, 0), -1)
 
-			# Detect aruco markers
-			logging.debug('\t> detect markers')
-
-			self.__aruco_detector.detect_markers(image)
-
 			# Fill camera frame background with timestamped image
 			self.background = image
 
-			# Clear former layers projection into camera frame
-			for layer_name, layer in self.layers.items():
+			# Read projection from the cache if required
+			if not self._read_projection_cache(image.timestamp):
+
+				# Detect aruco markers
+				logging.debug('\t> detect markers')
+
+				self.__aruco_detector.detect_markers(image)
 
-				# Initialize layer if needed
-				if layer.aoi_scene is None:
+				# Clear former layers projection into camera frame
+				self._clear_projection()
 
-					layer.aoi_scene = AOI2DScene.AOI2DScene()
+				# Project each aoi 3d scene into camera frame
+				for scene_name, scene in self.scenes.items():
 
-				else:
+					''' TODO: Enable aruco_aoi processing
+					if scene.aruco_aoi:
 
-					layer.aoi_scene.clear()
+						try:
 
-			# Project each aoi 3d scene into camera frame
-			for scene_name, scene in self.scenes.items():
+							# Build AOI scene directly from detected ArUco marker corners
+							self.layers[??].aoi_2d_scene |= scene.build_aruco_aoi_scene(self.__aruco_detector.detected_markers())
 
-				''' TODO: Enable aruco_aoi processing
-				if scene.aruco_aoi:
+						except ArFeatures.PoseEstimationFailed:
+
+							pass
+					'''
+
+					# Estimate scene pose from detected scene markers
+					logging.debug('\t> estimate %s scene pose', scene_name)
 
 					try:
 
-						# Build AOI scene directly from detected ArUco marker corners
-						self.layers[??].aoi_2d_scene |= scene.build_aruco_aoi_scene(self.__aruco_detector.detected_markers())
+						tvec, rmat, _ = scene.estimate_pose(self.__aruco_detector.detected_markers(), timestamp=image.timestamp)
 
-					except ArFeatures.PoseEstimationFailed:
+						# Project scene into camera frame according estimated pose
+						for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov, timestamp=image.timestamp):
 
-						pass
-				'''
+							logging.debug('\t> project %s scene %s layer', scene_name, layer_name)
 
-				# Estimate scene pose from detected scene markers
-				logging.debug('\t> estimate %s scene pose', scene_name)
+							try:
 
-				tvec, rmat, _ = scene.estimate_pose(self.__aruco_detector.detected_markers(), timestamp=self.timestamp)
+								# Update camera layer aoi
+								self.layers[layer_name].aoi_scene |= layer_projection
 
-				# Project scene into camera frame according estimated pose
-				for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov,
-				                                                  timestamp=self.timestamp):
+								# Timestamp camera layer
+								self.layers[layer_name].timestamp = image.timestamp
 
-					logging.debug('\t> project %s scene %s layer', scene_name, layer_name)
+							except KeyError:
 
-					try:
+								pass
 
-						# Update camera layer aoi
-						self.layers[layer_name].aoi_scene |= layer_projection
+						# Write projection into the cache if required
+						self._write_projection_cache(image.timestamp)
 
-						# Timestamp camera layer
-						self.layers[layer_name].timestamp = self.timestamp
+					except DataFeatures.TimestampedException as e:
 
-					except KeyError:
+						# Write exception into the cache if required
+						self._write_projection_cache(image.timestamp, e)
 
-						pass
+						# Raise exception
+						raise e
 
 	@DataFeatures.PipelineStepImage
 	def image(self, draw_detected_markers: dict = None, draw_scenes: dict = None,