From b3441d12929ee31908fd88138fae546495690459 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Mon, 29 Jan 2024 11:35:08 +0100 Subject: Making ArUcoMarkersGroup as PipelineStepObject. Markers size don't need to be equal anymore. --- src/argaze/ArFeatures.py | 21 +-- src/argaze/ArUcoMarkers/ArUcoCamera.py | 2 +- src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py | 170 ++++++++++----------- src/argaze/ArUcoMarkers/ArUcoScene.py | 12 +- src/argaze/DataFeatures.py | 6 - .../utils/demo_data/aruco_markers_group.json | 12 +- .../utils/demo_data/demo_aruco_markers_setup.json | 1 - 7 files changed, 117 insertions(+), 107 deletions(-) (limited to 'src') diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py index 0333565..0062c53 100644 --- a/src/argaze/ArFeatures.py +++ b/src/argaze/ArFeatures.py @@ -217,7 +217,7 @@ class ArLayer(DataFeatures.SharedObject, DataFeatures.PipelineStepObject): yield aoi_scan_path_analyzer_module_path, aoi_scan_path_analyzer.analysis def as_dict(self) -> dict: - """Export ArLayer attributes as dictionary.""" + """Export ArLayer properties as dictionary.""" return { **DataFeatures.PipelineStepObject.as_dict(self), @@ -408,7 +408,7 @@ class ArLayer(DataFeatures.SharedObject, DataFeatures.PipelineStepObject): Be aware that gaze movement positions are in the same range of value than aoi_scene size attribute. Parameters: - timestamp: ny number used to know when the given gaze position occurs + timestamp: method call timestamp (unit does'nt matter) gaze_movement: gaze movement to project """ @@ -914,7 +914,7 @@ class ArFrame(DataFeatures.SharedObject, DataFeatures.PipelineStepObject): Be aware that gaze positions are in the same range of value than size attribute. Parameters: - timestamp: any number used to know when the given gaze position occurs + timestamp: method call timestamp (unit does'nt matter) gaze_position: gaze position to project """ @@ -1143,7 +1143,7 @@ class ArScene(DataFeatures.PipelineStepObject): self.__distance_tolerance = value def as_dict(self) -> dict: - """Export ArScene attributes as dictionary.""" + """Export ArScene properties as dictionary.""" return { **DataFeatures.PipelineStepObject.as_dict(self), @@ -1436,7 +1436,7 @@ class ArCamera(ArFrame): yield scene_frame def as_dict(self) -> dict: - """Export ArCamera attributes as dictionary.""" + """Export ArCamera properties as dictionary.""" return { **ArFrame.as_dict(self), @@ -1450,7 +1450,7 @@ class ArCamera(ArFrame): """Detect AR features from image and project scenes into camera frame. Parameters: - timestamp: image time stamp (unit does'nt matter) + timestamp: method call timestamp (unit does'nt matter) image: image where to extract AR features """ @@ -1464,7 +1464,7 @@ class ArCamera(ArFrame): watch method needs to be called first. Parameters: - timestamp: any number used to know when the given gaze position occurs + timestamp: method call timestamp (unit does'nt matter) gaze_position: gaze position to project """ @@ -1501,18 +1501,21 @@ class ArCamera(ArFrame): pass @DataFeatures.PipelineStepMethod - def map(self): + def map(self, timestamp: int|float): """Project camera frame background into scene frames background. !!! warning watch method needs to be called first. + + Parameters: + timestamp: method call timestamp (unit does'nt matter) """ # Use camera frame lock feature with self._lock: # Project camera frame background into each scene frame if possible - for frame in self.scene_frames: + for frame in self.scene_frames(): # Is there an AOI inside camera frame layers projection which its name equals to a scene frame name? for camera_layer_name, camera_layer in self.__layers.items(): diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py index e4a9eeb..362e84b 100644 --- a/src/argaze/ArUcoMarkers/ArUcoCamera.py +++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py @@ -186,7 +186,7 @@ class ArUcoCamera(ArFeatures.ArCamera): ''' # Estimate scene pose from detected scene markers - tvec, rmat, _ = scene.estimate_pose(self.__aruco_detector.detected_markers) + tvec, rmat, _ = scene.estimate_pose(timestamp, self.__aruco_detector.detected_markers) # Project scene into camera frame according estimated pose for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov): diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py index 37bceec..2fd7eee 100644 --- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py +++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py @@ -14,6 +14,7 @@ import math import itertools import re +from argaze import DataFeatures from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator import numpy @@ -70,30 +71,32 @@ class Place(): marker: dict @dataclass -class ArUcoMarkersGroup(): - """Handle group of ArUco markers as one unique spatial entity and estimate its pose. - - Parameters: - marker_size: expected size of all markers in the group. - dictionary: expected dictionary of all markers in the group. - places: expected markers place. +class ArUcoMarkersGroup(DataFeatures.PipelineStepObject): """ + Handle group of ArUco markers as one unique spatial entity and estimate its pose. + """ + + def __init__(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None, places: dict = None, **kwargs): + """Initialize ArUcoMarkersGroup - marker_size: float = field(default=0.) - dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary) - places: dict = field(default_factory=dict) + Parameters: + dictionary: expected dictionary of all markers in the group. + places: expected markers place. + """ - def __post_init__(self): - """Init group pose and places pose.""" + # Init parent classes + DataFeatures.PipelineStepObject.__init__(self, **kwargs) - # Init pose data - self._translation = numpy.zeros(3) - self._rotation = numpy.zeros(3) + # Init private attributes + self.__dictionary = dictionary + self.__places = places + self.__translation = numpy.zeros(3) + self.__rotation = numpy.zeros(3) # Normalize places data new_places = {} - for identifier, data in self.places.items(): + for identifier, data in self.__places.items(): # Convert string identifier to int value if type(identifier) == str: @@ -122,10 +125,13 @@ class ArUcoMarkersGroup(): assert(is_rotation_matrix(rmat)) - new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size) + # Get marker size + size = numpy.array(data.pop('size')).astype(numpy.float32) + + new_marker = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, size) # Build marker corners thanks to translation vector and rotation matrix - place_corners = numpy.array([[-self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, -self.marker_size/2, 0], [-self.marker_size/2, -self.marker_size/2, 0]]) + place_corners = numpy.array([[-size/2, size/2, 0], [size/2, size/2, 0], [size/2, -size/2, 0], [-size/2, -size/2, 0]]) place_corners = place_corners.dot(rmat) + tvec new_places[identifier] = Place(place_corners, new_marker) @@ -140,7 +146,51 @@ class ArUcoMarkersGroup(): new_places[identifier] = data - self.places = new_places + self.__places = new_places + + @property + def dictionary(self) -> ArUcoMarkersDictionary.ArUcoMarkersDictionary: + """Get ArUco marker group ArUco dictionary.""" + return self.__dictionary + + @property + def places(self) -> dict: + """Get ArUco marker group places dictionary.""" + return self.__places + + @property + def identifiers(self) -> list: + """List place marker identifiers belonging to the group.""" + return list(self.__places.keys()) + + @property + def translation(self) -> numpy.array: + """Get ArUco marker group translation vector.""" + return self.__translation + + @translation.setter + def translation(self, tvec): + """Set ArUco marker group translation vector.""" + self.__translation = tvec + + @property + def rotation(self) -> numpy.array: + """Get ArUco marker group rotation matrix.""" + return self.__translation + + @rotation.setter + def rotation(self, rmat): + """Set ArUco marker group rotation matrix.""" + self.__rotation = rmat + + def as_dict(self) -> dict: + """Export ArUco marker group properties as dictionary.""" + + return { + **DataFeatures.PipelineStepObject.as_dict(self), + "dictionary": self.__dictionary, + "places": self.__places + } @classmethod def from_obj(self, obj_filepath: str) -> ArUcoMarkersGroupType: @@ -154,7 +204,6 @@ class ArUcoMarkersGroup(): """ - new_marker_size = 0 new_dictionary = None new_places = {} @@ -246,31 +295,25 @@ class ArUcoMarkersGroup(): # Check axis size: they should be almost equal if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3): - current_marker_size = place_x_axis_norm - - # Check that all markers size are almost equal - if new_marker_size > 0: + new_marker_size = place_x_axis_norm - if not math.isclose(current_marker_size, new_marker_size, rel_tol=1e-3): + else: - raise ValueError('Markers size should be almost equal.') + raise ValueError(f'{new_dictionary}#{identifier}_Marker is not a square.') - new_marker_size = current_marker_size - - # Create a new place related to a new marker + # Create a new place related to a new marker new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) new_places[identifier] = Place(cw_corners, new_marker) except IOError: raise IOError(f'File not found: {obj_filepath}') - return ArUcoMarkersGroup(new_marker_size, new_dictionary, new_places) + return ArUcoMarkersGroup(new_dictionary, new_places) @classmethod def from_json(self, json_filepath: str) -> ArUcoMarkersGroupType: """Load ArUco markers group from .json file.""" - new_marker_size = 0 new_dictionary = None new_places = {} @@ -278,31 +321,10 @@ class ArUcoMarkersGroup(): data = json.load(configuration_file) - new_marker_size = data.pop('marker_size') new_dictionary = data.pop('dictionary') new_places = data.pop('places') - return ArUcoMarkersGroup(new_marker_size, new_dictionary, new_places) - - def __str__(self) -> str: - """String display""" - - output = f'\n\tDictionary: {self.dictionary}' - - output += f'\n\tMarker size: {self.marker_size} cm' - - output += '\n\n\tPlaces:' - for identifier, place in self.places.items(): - output += f'\n\t\t- {identifier}:' - output += f'\n{place.corners}' - - return output - - @property - def identifiers(self) -> list: - """List place marker identifiers belonging to the group.""" - - return list(self.places.keys()) + return ArUcoMarkersGroup(new_dictionary, new_places) def filter_markers(self, detected_markers: dict) -> Tuple[dict, dict]: """Sort markers belonging to the group from given detected markers dict (cf ArUcoDetector.detect_markers()). @@ -317,7 +339,7 @@ class ArUcoMarkersGroup(): for (marker_id, marker) in detected_markers.items(): - if marker_id in self.places.keys(): + if marker_id in self.__places.keys(): group_markers[marker_id] = marker @@ -348,7 +370,7 @@ class ArUcoMarkersGroup(): try: - place = self.places[identifier] + place = self.__places[identifier] for marker_corner in marker.corners: markers_corners_2d.append(list(marker_corner)) @@ -370,39 +392,17 @@ class ArUcoMarkersGroup(): rvec, tvec = cv2.solvePnPRefineVVS(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), rvec, tvec) - self._translation = tvec.T - self._rotation = rvec.T - - return success, self._translation, self._rotation - - @property - def translation(self) -> numpy.array: - """Access to group translation vector.""" - - return self._translation - - @translation.setter - def translation(self, tvec): - - self._translation = tvec - - @property - def rotation(self) -> numpy.array: - """Access to group rotation matrix.""" - - return self._translation - - @rotation.setter - def rotation(self, rmat): + self.__translation = tvec.T + self.__rotation = rvec.T - self._rotation = rmat + return success, self.__translation, self.__rotation def draw_axes(self, image: numpy.array, K, D, thickness: int = 0, length: float = 0): """Draw group axes.""" try: axisPoints = numpy.float32([[length, 0, 0], [0, length, 0], [0, 0, length], [0, 0, 0]]).reshape(-1, 3) - axisPoints, _ = cv2.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + axisPoints, _ = cv2.projectPoints(axisPoints, self.__rotation, self.__translation, numpy.array(K), numpy.array(D)) axisPoints = axisPoints.astype(int) cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red) @@ -418,11 +418,11 @@ class ArUcoMarkersGroup(): l = self.marker_size / 2 - for identifier, place in self.places.items(): + for identifier, place in self.__places.items(): try: - placePoints, _ = cv2.projectPoints(place.corners, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + placePoints, _ = cv2.projectPoints(place.corners, self.__rotation, self.__translation, numpy.array(K), numpy.array(D)) placePoints = placePoints.astype(int) cv2.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), color, border_size) @@ -462,9 +462,9 @@ class ArUcoMarkersGroup(): v_count = 0 - for p, (identifier, place) in enumerate(self.places.items()): + for p, (identifier, place) in enumerate(self.__places.items()): - file.write(f'o {self.dictionary.name}#{identifier}_Marker\n') + file.write(f'o {self.__dictionary.name}#{identifier}_Marker\n') vertices = '' diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index f1bdf5e..84de39e 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -39,6 +39,11 @@ class ArUcoScene(ArFeatures.ArScene): # Init private attribute self.__aruco_markers_group = aruco_markers_group + # Edit pipeline step objects parent + if self.__aruco_markers_group is not None: + + self.__aruco_markers_group.parent = self + @property def aruco_markers_group(self) -> ArUcoMarkersGroup.ArUcoMarkersGroup: """Get ArUco scene markers group object.""" @@ -94,9 +99,14 @@ class ArUcoScene(ArFeatures.ArScene): **temp_scene_data \ ) - def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, dict]: + @DataFeatures.PipelineStepMethod + def estimate_pose(self, timestamp: int|float, detected_markers: dict) -> Tuple[numpy.array, numpy.array, dict]: """Estimate scene pose from detected ArUco markers. + Parameters: + timestamp: method call timestamp (unit does'nt matter) + detected_markers: dictionary with all detected markers + Returns: scene translation vector scene rotation matrix diff --git a/src/argaze/DataFeatures.py b/src/argaze/DataFeatures.py index e36b52f..688ad4a 100644 --- a/src/argaze/DataFeatures.py +++ b/src/argaze/DataFeatures.py @@ -609,12 +609,6 @@ class PipelineStepObject(): yield name, getattr(self, name) -def PipelineStepProperty(method): - - print('PipelineStepProperty', method) - - return method - def PipelineStepMethod(method): """Define a decorator use into PipelineStepObject class to declare pipeline method. diff --git a/src/argaze/utils/demo_data/aruco_markers_group.json b/src/argaze/utils/demo_data/aruco_markers_group.json index d824ee9..f053016 100644 --- a/src/argaze/utils/demo_data/aruco_markers_group.json +++ b/src/argaze/utils/demo_data/aruco_markers_group.json @@ -4,19 +4,23 @@ "places": { "0": { "translation": [-2.5, 17.5, 0], - "rotation": [0.0, 0.0, 0.0] + "rotation": [0.0, 0.0, 0.0], + "size": 5 }, "1": { "translation": [27.5, 17.5, 0], - "rotation": [0.0, 0.0, 0.0] + "rotation": [0.0, 0.0, 0.0], + "size": 5 }, "2": { "translation": [-2.5, -2.5, 0], - "rotation": [0.0, 0.0, 0.0] + "rotation": [0.0, 0.0, 0.0], + "size": 5 }, "3": { "translation": [27.5, -2.5, 0], - "rotation": [0.0, 0.0, 0.0] + "rotation": [0.0, 0.0, 0.0], + "size": 5 } } } \ No newline at end of file diff --git a/src/argaze/utils/demo_data/demo_aruco_markers_setup.json b/src/argaze/utils/demo_data/demo_aruco_markers_setup.json index 8e3c5ea..53f0bc3 100644 --- a/src/argaze/utils/demo_data/demo_aruco_markers_setup.json +++ b/src/argaze/utils/demo_data/demo_aruco_markers_setup.json @@ -3,7 +3,6 @@ "size": [1280, 720], "aruco_detector": { "dictionary": "DICT_APRILTAG_16h5", - "marker_size": 5, "parameters": { "useAruco3Detection": 1 } -- cgit v1.1