From 09a18f98dd038d2a79f35b56c991b237d9add015 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Tue, 29 Nov 2022 20:53:07 +0100 Subject: Loading places from .obj file. --- src/argaze/ArGazeScene.py | 15 +- src/argaze/ArUcoMarkers/ArUcoScene.py | 179 ++++++++++++++++----- .../utils/tobii_segment_argaze_scene_export.py | 4 +- 3 files changed, 155 insertions(+), 43 deletions(-) (limited to 'src') diff --git a/src/argaze/ArGazeScene.py b/src/argaze/ArGazeScene.py index 56ac18e..0eb4ebc 100644 --- a/src/argaze/ArGazeScene.py +++ b/src/argaze/ArGazeScene.py @@ -35,6 +35,9 @@ class ArGazeScene(): aruco_scene: ArUcoScene.ArUcoScene = field(init=False, default_factory=ArUcoScene.ArUcoScene) """ArUco scene ...""" + aoi_scene: AOI3DScene.AOI3DScene = field(init=False, default_factory=AOI3DScene.AOI3DScene) + """AOI 3D scene ...""" + def __init__(self, **kwargs): self.aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(kwargs.pop('aruco_dictionary')) @@ -45,18 +48,26 @@ class ArGazeScene(): self.aruco_tracker = ArUcoTracker.ArUcoTracker(self.aruco_dictionary, self.aruco_marker_size, self.aruco_camera, **kwargs.pop('aruco_tracker')) + # Check aruco_scene.places value type + aruco_scene_places_value = kwargs['aruco_scene']['places'] + + # str: relative path to .obj file + if type(aruco_scene_places_value) == str: + + kwargs['aruco_scene']['places'] = os.path.join(self.__current_directory, aruco_scene_places_value) + self.aruco_scene = ArUcoScene.ArUcoScene(self.aruco_dictionary, self.aruco_marker_size, **kwargs.pop('aruco_scene')) # Check aoi_scene value type aoi_scene_value = kwargs.pop('aoi_scene') - # Relative path to a .obj file + # str: relative path to .obj file if type(aoi_scene_value) == str: obj_filepath = os.path.join(self.__current_directory, aoi_scene_value) self.aoi_scene = AOI3DScene.AOI3DScene.from_obj(obj_filepath) - # Dict of all AOI + # dict: all AOI else: self.aoi_scene = AOI3DScene.AOI3DScene(aoi_scene_value) diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index 2134cf7..857ebf4 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -5,6 +5,7 @@ from dataclasses import dataclass, field import json import math import itertools +import re from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoCamera @@ -15,49 +16,62 @@ import cv2.aruco as aruco ArUcoSceneType = TypeVar('ArUcoScene', bound="ArUcoScene") # Type definition for type annotation convenience -@dataclass +@dataclass(frozen=True) class Place(): """Define a place as a pose and a marker.""" translation: numpy.array - """Position in set referential.""" + """Position in scene referential.""" rotation: numpy.array - """Rotation in set referential.""" + """Rotation in scene referential.""" marker: dict """ArUco marker linked to the place.""" -@dataclass class ArUcoScene(): """Define abstract class to handle group of ArUco markers as one unique spatial entity and estimate its pose.""" - places: dict = field(init=False, default_factory=dict) - """All named places of the set and their ArUco markers.""" - - angle_tolerance: float = field(init=False) + angle_tolerance: float """Angle error tolerance allowed to validate place pose in degree.""" - distance_tolerance: float = field(init=False) + distance_tolerance: float """Distance error tolerance allowed to validate place pose in centimeter.""" - def __init__(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary, marker_size: float, **kwargs): - """Define set from a .json file.""" + def __init__(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary, marker_size: float, angle_tolerance: float, distance_tolerance: float, places: dict | str = None): + """Define scene attributes.""" self.__dictionary = dictionary self.__marker_size = marker_size - # Load places - self.places = {} - for name, place in kwargs['places'].items(): - marker = ArUcoMarker.ArUcoMarker(self.__dictionary, place['marker'], self.__marker_size) - self.places[name] = Place(numpy.array(place['translation']).astype(numpy.float32), numpy.array(place['rotation']).astype(numpy.float32), marker) + self.angle_tolerance = angle_tolerance + self.distance_tolerance = distance_tolerance + + # NEVER USE {} as default function argument + self.places = places + + @property + def places(self) -> dict: + """All named places of the scene and their ArUco markers.""" + + return self.__places + + @places.setter + def places(self, places: dict | str): + + # str: path to .obj file + if type(places) == str: - # Load angle tolerance - self.angle_tolerance = kwargs['angle_tolerance'] + self.__load_places_from_obj(places) - # Load distance tolerance - self.distance_tolerance = kwargs['distance_tolerance'] + # dict: all places + else: + + self.__places = {} + + for name, place in places.items(): + marker = ArUcoMarker.ArUcoMarker(self.__dictionary, place['marker'], self.__marker_size) + self.__places[name] = Place(numpy.array(place['translation']).astype(numpy.float32), numpy.array(place['rotation']).astype(numpy.float32), marker) # Init pose data self._translation = numpy.zeros(3) @@ -67,17 +81,17 @@ class ArUcoScene(): # Process markers ids to speed up further calculations self.__identifier_cache = {} - for name, place in self.places.items(): + for name, place in self.__places.items(): self.__identifier_cache[place.marker.identifier] = name # Process each place pose to speed up further calculations self.__translation_cache = {} - for name, place in self.places.items(): + for name, place in self.__places.items(): self.__translation_cache[name] = place.translation # Process each place rotation matrix to speed up further calculations self.__rotation_cache = {} - for name, place in self.places.items(): + for name, place in self.__places.items(): # Create intrinsic rotation matrix R = self.__make_rotation_matrix(*place.rotation) @@ -89,7 +103,7 @@ class ArUcoScene(): # Process axis-angle between place combination to speed up further calculations self.__angle_cache = {} - for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.places.items(), 2): + for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.__places.items(), 2): A = self.__rotation_cache[A_name] B = self.__rotation_cache[B_name] @@ -120,7 +134,7 @@ class ArUcoScene(): # Process distance between each place combination to speed up further calculations self.__distance_cache = {} - for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.places.items(), 2): + for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.__places.items(), 2): A = self.__translation_cache[A_name] B = self.__translation_cache[B_name] @@ -145,7 +159,7 @@ class ArUcoScene(): with open(json_filepath) as configuration_file: return ArUcoScene(**json.load(configuration_file)) - + def __str__(self) -> str: """String display""" @@ -181,6 +195,93 @@ class ArUcoScene(): return list(self.__identifier_cache.keys()) + def __load_places_from_obj(self, obj_filepath: str) -> dict: + """Load places from .obj file.""" + + self.__places = {} + + # regex rules for .obj file parsing + OBJ_RX_DICT = { + 'comment': re.compile(r'#(.*)\n'), + 'name': re.compile(r'o (\w+)(.*)\n'), + 'vertice': re.compile(r'v ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), + 'normal': re.compile(r'vn ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), + 'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n') + } + + # regex .obj line parser + def __parse_obj_line(line): + + for key, rx in OBJ_RX_DICT.items(): + match = rx.search(line) + if match: + return key, match + + # if there are no matches + return None, None + + # start parsing + try: + + name = None + vertices = [] + normals = {} + faces = {} + + # open the file and read through it line by line + with open(obj_filepath, 'r') as file: + + line = file.readline() + + while line: + + # at each line check for a match with a regex + key, match = __parse_obj_line(line) + + # extract comment + if key == 'comment': + pass + + # extract place name + elif key == 'name': + + name = str(match.group(1)) + + # fill vertices array + elif key == 'vertice': + + vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])) + + # extract normal + elif key == 'normal': + + normals[name] = numpy.array([float(match.group(1)), float(match.group(2)), float(match.group(3))]) + + # extract aoi3D vertice id + elif key == 'face': + + faces[name] = [int(i) for i in match.group(1).split()] + + # go to next line + line = file.readline() + + file.close() + + # retreive all place vertices + for name, face in faces.items(): + + center = numpy.array([ vertices[i-1] for i in face ]).mean(axis=0) + normal = normals[name] + + # WARNING: here we set marker identifier depending on the place position in the file + # TODO: extract identifiers from name + marker = ArUcoMarker.ArUcoMarker(self.__dictionary, len(self.__places), self.__marker_size) + + self.__places[name] = Place(center, normal, marker) + + except IOError: + raise IOError(f'File not found: {obj_filepath}') + def __make_rotation_matrix(self, x, y, z): # Create rotation matrix around x axis @@ -209,13 +310,13 @@ class ArUcoScene(): def __normalise_place_pose(self, name, place, F): - # Transform place rotation into set rotation vector + # Transform place rotation into scene rotation vector R = self.__rotation_cache[name] rvec, _ = cv.Rodrigues(F.dot(R)) #print(f'{name} rotation vector: {rvec[0][0]:3f} {rvec[1][0]:3f} {rvec[2][0]:3f}') - # Transform place translation into set translation vector + # Transform place translation into scene translation vector OF = place.translation T = self.__translation_cache[name] FC = R.dot(F.dot(-T)) @@ -227,7 +328,7 @@ class ArUcoScene(): return rvec, tvec def estimate_pose(self, tracked_markers) -> Tuple[numpy.array, numpy.array, bool, int, dict]: - """Estimate set pose from tracked markers (cf ArUcoTracker.track()) + """Estimate scene pose from tracked markers (cf ArUcoTracker.track()) * **Returns:** - translation vector @@ -262,10 +363,10 @@ class ArUcoScene(): #print('-------------- ArUcoScene pose estimation --------------') - # Pose validity checking is'nt possible when only one place of the set is tracked + # Pose validity checking is'nt possible when only one place of the scene is tracked if len(tracked_places.keys()) == 1: - # Get set pose from to the unique place pose + # Get scene pose from to the unique place pose name, place = tracked_places.popitem() F, _ = cv.Rodrigues(place.rotation) @@ -394,10 +495,10 @@ class ArUcoScene(): @property def translation(self) -> numpy.array: - """Access to set translation vector. + """Access to scene translation vector. .. warning:: - Setting set translation vector implies succeded status to be True and validity score to be 0.""" + Setting scene translation vector implies succeded status to be True and validity score to be 0.""" return self._translation @@ -410,10 +511,10 @@ class ArUcoScene(): @property def rotation(self) -> numpy.array: - """Access to set rotation vector. + """Access to scene rotation vector. .. warning:: - Setting set rotation vector implies succeded status to be True and validity score to be 0.""" + Setting scene rotation vector implies succeded status to be True and validity score to be 0.""" return self._translation @@ -426,18 +527,18 @@ class ArUcoScene(): @property def succeded(self) -> bool: - """Access to set pose estimation succeded status.""" + """Access to scene pose estimation succeded status.""" return self._succeded @property def validity(self) -> int: - """Access to set pose estimation validity score.""" + """Access to scene pose estimation validity score.""" return self._validity def draw(self, frame, K, D, draw_places=True): - """Draw set axis and places.""" + """Draw scene axis and places.""" l = self.__marker_size / 2 ll = self.__marker_size @@ -460,7 +561,7 @@ class ArUcoScene(): # Draw places (optional) if draw_places: - for name, place in self.places.items(): + for name, place in self.__places.items(): if name != "top": continue diff --git a/src/argaze/utils/tobii_segment_argaze_scene_export.py b/src/argaze/utils/tobii_segment_argaze_scene_export.py index ae42d7c..a038e34 100644 --- a/src/argaze/utils/tobii_segment_argaze_scene_export.py +++ b/src/argaze/utils/tobii_segment_argaze_scene_export.py @@ -86,12 +86,12 @@ def main(): # Load a tobii segment video tobii_segment_video = tobii_segment.load_video() - print(f'Video properties:\n\tduration: {tobii_segment_video.duration/1e6} s\n\twidth: {tobii_segment_video.width} px\n\theight: {tobii_segment_video.height} px') + print(f'\nVideo properties:\n\tduration: {tobii_segment_video.duration/1e6} s\n\twidth: {tobii_segment_video.width} px\n\theight: {tobii_segment_video.height} px') # Load a tobii segment data tobii_segment_data = tobii_segment.load_data() - print(f'Loaded data count:') + print(f'\nLoaded data count:') for name in tobii_segment_data.keys(): print(f'\t{name}: {len(tobii_segment_data[name])} data') -- cgit v1.1