aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorThéo de la Hogue2024-04-16 18:31:39 +0200
committerThéo de la Hogue2024-04-16 18:31:39 +0200
commitfad3be2544b4abe01660a5be41b8f38e8f76dffb (patch)
tree59f6440088d799e32b574924f973e8e0028bf64d /src
parent3ce2e25d6f0417bc78bc10c8fd3cbd2ecaf27407 (diff)
downloadargaze-fad3be2544b4abe01660a5be41b8f38e8f76dffb.zip
argaze-fad3be2544b4abe01660a5be41b8f38e8f76dffb.tar.gz
argaze-fad3be2544b4abe01660a5be41b8f38e8f76dffb.tar.bz2
argaze-fad3be2544b4abe01660a5be41b8f38e8f76dffb.tar.xz
Adding projection cache feature.
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArFeatures.py157
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoCamera.py83
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,