From fad3be2544b4abe01660a5be41b8f38e8f76dffb Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Tue, 16 Apr 2024 18:31:39 +0200 Subject: Adding projection cache feature. --- src/argaze/ArFeatures.py | 157 +++++++++++++++++++++++++++++++++ src/argaze/ArUcoMarkers/ArUcoCamera.py | 83 ++++++++--------- 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, -- cgit v1.1