diff options
Diffstat (limited to 'src/argaze/ArUcoMarkers/ArUcoScene.py')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 297 |
1 files changed, 174 insertions, 123 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index 795bfe4..15e7a35 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -38,7 +38,7 @@ class Place(): class ArUcoScene(): """Handle group of ArUco markers as one unique spatial entity and estimate its pose.""" - def __init__(self, marker_size: float, places: dict | str, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None): + def __init__(self, places: dict | str, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None, marker_size: float = 0.): """Define scene attributes.""" # Init expected marker size @@ -62,13 +62,26 @@ class ArUcoScene(): self.places = places @property + def marker_size(self) -> float: + """Expected size of all markers in the scene.""" + + return self.__marker_size + + @property def places(self) -> dict: - """All named places of the scene and their ArUco markers.""" + """All places of the scene and their related ArUco markers.""" return self.__places @places.setter def places(self, data: dict | str): + """Load places from dict or .obj file. + + .. warning:: .obj file loading overwrites marker_size attribute + + .. warning:: in .obj file, 'o' tag string format should be DICTIONARY#IDENTIFIER_NAME + + """ # str: path to .obj file if type(data) == str: @@ -80,29 +93,27 @@ class ArUcoScene(): self.__places = {} - for name, place in data.items(): + for identifier, place in data.items(): + + # Convert string identifier to int value + identifier = int(identifier) tvec = numpy.array(place['translation']).astype(numpy.float32) rmat = self.__make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32) - marker = ArUcoMarker.ArUcoMarker(self.__dictionary, place['marker'], self.__marker_size) + marker = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size) - self.__places[name] = Place(tvec, rmat, marker) + self.__places[identifier] = Place(tvec, rmat, marker) # Init pose data self._translation = numpy.zeros(3) self._rotation = numpy.zeros(3) - # Process markers ids to speed up further calculations - self.__identifier_cache = {} - for name, place in self.__places.items(): - self.__identifier_cache[place.marker.identifier] = name - # 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_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2): - A = self.__places[A_name].rotation - B = self.__places[B_name].rotation + A = self.__places[A_identifier].rotation + B = self.__places[B_identifier].rotation if numpy.array_equal(A, B): @@ -117,34 +128,34 @@ class ArUcoScene(): angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) try: - self.__angle_cache[A_name][B_name] = angle + self.__angle_cache[A_identifier][B_identifier] = angle except: - self.__angle_cache[A_name] = {B_name: angle} + self.__angle_cache[A_identifier] = {B_identifier: angle} try: - self.__angle_cache[B_name][A_name] = angle + self.__angle_cache[B_identifier][A_identifier] = angle except: - self.__angle_cache[B_name] = {A_name: angle} + self.__angle_cache[B_identifier] = {A_identifier: angle} # 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_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2): - A = self.__places[A_name].translation - B = self.__places[B_name].translation + A = self.__places[A_identifier].translation + B = self.__places[B_identifier].translation # Calculate axis-angle representation of AB rotation matrix distance = numpy.linalg.norm(B - A) try: - self.__distance_cache[A_name][B_name] = distance + self.__distance_cache[A_identifier][B_identifier] = distance except: - self.__distance_cache[A_name] = {B_name: distance} + self.__distance_cache[A_identifier] = {B_identifier: distance} try: - self.__distance_cache[B_name][A_name] = distance + self.__distance_cache[B_identifier][A_identifier] = distance except: - self.__distance_cache[B_name] = {A_name: distance} + self.__distance_cache[B_identifier] = {A_identifier: distance} @classmethod def from_json(self, json_filepath) -> ArUcoSceneType: @@ -160,32 +171,28 @@ class ArUcoScene(): output = f'\n\n\tDictionary: {self.__dictionary.name}' output += '\n\n\tPlaces:' - for name, place in self.__places.items(): - output += f'\n\t\t- {name}:' + for identifier, place in self.__places.items(): + output += f'\n\t\t- {identifier}:' output += f'\n{place.translation}' output += f'\n{place.rotation}' - output += '\n\n\tIdentifier cache:' - for i, name in self.__identifier_cache.items(): - output += f'\n\t\t- {i}: {name}' - output += '\n\n\tAngle cache:' - for A_name, A_angle_cache in self.__angle_cache.items(): - for B_name, angle in A_angle_cache.items(): - output += f'\n\t\t- {A_name}/{B_name}: {angle:3f}' + for A_identifier, A_angle_cache in self.__angle_cache.items(): + for B_identifier, angle in A_angle_cache.items(): + output += f'\n\t\t- {A_identifier}/{B_identifier}: {angle:3f}' output += '\n\n\tDistance cache:' - for A_name, A_distance_cache in self.__distance_cache.items(): - for B_name, distance in A_distance_cache.items(): - output += f'\n\t\t- {A_name}/{B_name}: {distance:3f}' + for A_identifier, A_distance_cache in self.__distance_cache.items(): + for B_identifier, distance in A_distance_cache.items(): + output += f'\n\t\t- {A_identifier}/{B_identifier}: {distance:3f}' return output @property def identifiers(self) -> list: - """List all makers identifier belonging to the scene.""" + """List place maker identifiers belonging to the scene.""" - return list(self.__identifier_cache.keys()) + return list(self.__places.keys()) def __make_rotation_matrix(self, x, y, z): @@ -208,12 +215,9 @@ class ArUcoScene(): return Rx.dot(Ry.dot(Rz)) def __load_places_from_obj(self, obj_filepath: str) -> dict: - """Load places from .obj file. - - .. warning:: 'o' tag string format should be DICTIONARY#IDENTIFIER_NAME - """ self.__places = {} + self.__marker_size = 0 # Regex rules for .obj file parsing OBJ_RX_DICT = { @@ -238,7 +242,7 @@ class ArUcoScene(): # Start parsing try: - name = None + identifier = None vertices = [] markers = {} normals = {} @@ -268,12 +272,11 @@ class ArUcoScene(): # Check that marker dictionary is like the scene dictionary if dictionary == self.__dictionary.name: - name = f'{dictionary}#{identifier}' # ignore last part - markers[name] = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size) + markers[identifier] = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size) else: - raise NameError(f'Marker#{identifier} dictionary is not {self.__dictionary.name}') + raise NameError(f'Marker {identifier} dictionary is not {self.__dictionary.name}') # Fill vertices array elif key == 'vertice': @@ -283,12 +286,12 @@ class ArUcoScene(): # Extract normal to calculate rotation matrix elif key == 'normal': - normals[name] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]) + normals[identifier] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]) # Extract vertice ids elif key == 'face': - faces[name] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))] + faces[identifier] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))] # Go to next line line = file.readline() @@ -296,7 +299,7 @@ class ArUcoScene(): file.close() # Retreive marker vertices thanks to face vertice ids - for name, face in faces.items(): + for identifier, face in faces.items(): # Gather place corners from counter clockwise ordered face vertices corners = numpy.array([ vertices[i-1] for i in face ]) @@ -306,24 +309,40 @@ class ArUcoScene(): # Edit place axis from corners positions place_x_axis = corners[1:3].mean(axis=0) - Tp - place_x_axis = place_x_axis / numpy.linalg.norm(place_x_axis) + place_x_axis_norm = numpy.linalg.norm(place_x_axis) + place_x_axis = place_x_axis / place_x_axis_norm place_y_axis = corners[2:4].mean(axis=0) - Tp - place_y_axis = place_y_axis / numpy.linalg.norm(place_y_axis) + place_y_axis_norm = numpy.linalg.norm(place_y_axis) + place_y_axis = place_y_axis / place_y_axis_norm - place_z_axis = normals[name] + place_z_axis = normals[identifier] # Edit rotation (Rp) allowing to transform world axis (W) into place axis (P) W = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) P = numpy.array([place_x_axis, place_y_axis, place_z_axis]) Rp = W.dot(P.T) - self.__places[name] = Place(Tp, Rp, markers[name]) + # 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*2 + + # Check that all markers size are almost equal + if self.__marker_size > 0: + + if not math.isclose(current_marker_size, self.__marker_size, rel_tol=1e-3): + + raise ValueError('Markers size should be almost equal.') + + self.__marker_size = current_marker_size + + self.__places[identifier] = Place(Tp, Rp, markers[identifier]) except IOError: raise IOError(f'File not found: {obj_filepath}') - def filter_markers(self, detected_markers) -> Tuple[dict, dict]: + def filter_markers(self, detected_markers: dict) -> Tuple[dict, dict]: """Sort markers belonging to the scene from given detected markers dict (cf ArUcoDetector.detect_markers()). * **Returns:** @@ -336,17 +355,17 @@ class ArUcoScene(): for (marker_id, marker) in detected_markers.items(): - try: - name = self.__identifier_cache[marker_id] - scene_markers[name] = marker + if marker_id in self.__places.keys(): - except KeyError: + scene_markers[marker_id] = marker + + else: remaining_markers[marker_id] = marker return scene_markers, remaining_markers - def check_markers_consistency(self, scene_markers, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict]: + def check_markers_consistency(self, scene_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict]: """Evaluate if given markers configuration match related places configuration. * **Returns:** @@ -358,42 +377,48 @@ class ArUcoScene(): consistent_markers = {} unconsistencies = {} - for (A_name, A_marker), (B_name, B_marker) in itertools.combinations(scene_markers.items(), 2): + for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(scene_markers.items(), 2): + + try: - # Rotation matrix from A marker to B marker - AB = B_marker.rotation.dot(A_marker.rotation.T) + # Rotation matrix from A marker to B marker + AB = B_marker.rotation.dot(A_marker.rotation.T) - # Calculate axis-angles representation of AB rotation matrix - angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) - expected_angle = self.__angle_cache[A_name][B_name] + # Calculate axis-angles representation of AB rotation matrix + angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) + expected_angle = self.__angle_cache[A_identifier][B_identifier] - # Calculate distance between A marker center and B marker center - distance = numpy.linalg.norm(A_marker.translation - B_marker.translation) - expected_distance = self.__distance_cache[A_name][B_name] + # Calculate distance between A marker center and B marker center + distance = numpy.linalg.norm(A_marker.translation - B_marker.translation) + expected_distance = self.__distance_cache[A_identifier][B_identifier] - # Check angle and distance according given tolerance then normalise marker pose - consistent_angle = math.isclose(angle, expected_angle, abs_tol=angle_tolerance) - consistent_distance = math.isclose(distance, expected_distance, abs_tol=distance_tolerance) + # Check angle and distance according given tolerance then normalise marker pose + consistent_angle = math.isclose(angle, expected_angle, abs_tol=angle_tolerance) + consistent_distance = math.isclose(distance, expected_distance, abs_tol=distance_tolerance) - if consistent_angle and consistent_distance: + if consistent_angle and consistent_distance: - if A_name not in consistent_markers.keys(): + if A_identifier not in consistent_markers.keys(): - # Remember this marker is already validated - consistent_markers[A_name] = A_marker + # Remember this marker is already validated + consistent_markers[A_identifier] = A_marker - if B_name not in consistent_markers.keys(): + if B_identifier not in consistent_markers.keys(): - # Remember this marker is already validated - consistent_markers[B_name] = B_marker + # Remember this marker is already validated + consistent_markers[B_identifier] = B_marker - else: + else: - if not consistent_angle: - unconsistencies[f'{A_name}/{B_name} angle'] = angle - - if not consistent_distance: - unconsistencies[f'{A_name}/{B_name} distance'] = distance + if not consistent_angle: + unconsistencies[f'{A_identifier}/{B_identifier} angle'] = angle + + if not consistent_distance: + unconsistencies[f'{A_identifier}/{B_identifier} distance'] = distance + + except KeyError: + + raise ValueError(f'Marker {A_identifier} or {B_identifier} don\'t belong to the scene.') # Gather unconsistent markers unconsistent_markers = {} @@ -406,16 +431,70 @@ class ArUcoScene(): return consistent_markers, unconsistent_markers, unconsistencies - def estimate_pose_from_axis_markers(self, axis_markers) -> Tuple[numpy.array, numpy.array]: - """Calculate tranformations (rotation and translation) from a list of 3 (name, marker) tuples defining an orthogonal axis.""" + def estimate_pose_from_single_marker(self, marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: + """Calculate rotation and translation that move a marker to its place.""" + + # Get the place related to the given marker + try: + + place = self.__places[marker.identifier] + + # Rotation matrix that transform marker to related place + self._rotation = marker.rotation.dot(place.rotation.T) + + # Translation vector that transform marker to related place + self._translation = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) + + return self._translation, self._rotation + + except KeyError: + + raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the scene.') + + def estimate_pose_from_markers(self, markers: dict) -> Tuple[numpy.array, numpy.array]: + """Calculate average rotation and translation that move markers to their related places.""" + + rotations = [] + translations = [] + + for identifier, marker in markers.items(): + + try: + + place = self.__places[identifier] + + # Rotation matrix that transform marker to related place + R = marker.rotation.dot(place.rotation.T) + + # Translation vector that transform marker to related place + T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) + + rotations.append(R) + translations.append(T) + + except KeyError: + + raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the scene.') + + # Consider ArUcoScene rotation as the mean of all marker rotations + # !!! WARNING !!! This is a bad hack : processing rotations average is a very complex problem that needs to well define the distance calculation method before. + self._rotation = numpy.mean(numpy.array(rotations), axis=0) + + # Consider ArUcoScene translation as the mean of all marker translations + self._translation = numpy.mean(numpy.array(translations), axis=0) + + return self._translation, self._rotation - O_name, O_marker = axis_markers[0] - A_name, A_marker = axis_markers[1] - B_name, B_marker = axis_markers[2] + def estimate_pose_from_axis_markers(self, origin_marker: ArUcoMarker.ArUcoMarker, horizontal_axis_marker: ArUcoMarker.ArUcoMarker, vertical_axis_marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: + """Calculate rotation and translation from 3 markers defining an orthogonal axis.""" - O_place = self.__places[O_name] - A_place = self.__places[A_name] - B_place = self.__places[B_name] + O_marker = origin_marker + A_marker = horizontal_axis_marker + B_marker = vertical_axis_marker + + O_place = self.__places[O_marker.identifier] + A_place = self.__places[A_marker.identifier] + B_place = self.__places[B_marker.identifier] # Place axis OA = A_place.translation - O_place.translation @@ -450,34 +529,6 @@ class ArUcoScene(): return self._translation, self._rotation - def estimate_pose_from_any_markers(self, consistent_markers) -> Tuple[numpy.array, numpy.array]: - """Calculate transformations (rotation and translation) that move each marker to its related place.""" - - rotations = [] - translations = [] - - for (name, marker) in consistent_markers.items(): - - place = self.__places[name] - - # Rotation matrix that transform marker to related place - R = marker.rotation.dot(place.rotation.T) - - # Translation vector that transform marker to related place - T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) - - rotations.append(R) - translations.append(T) - - # Consider ArUcoScene rotation as the mean of all marker rotations - # !!! WARNING !!! This is a bad hack : processing rotations average is a very complex problem that needs to well define the distance calculation method before. - self._rotation = numpy.mean(numpy.array(rotations), axis=0) - - # Consider ArUcoScene translation as the mean of all marker translations - self._translation = numpy.mean(numpy.array(translations), axis=0) - - return self._translation, self._rotation - @property def translation(self) -> numpy.array: """Access to scene translation vector.""" @@ -529,12 +580,12 @@ class ArUcoScene(): n = 95 * consistency if consistency < 2 else 0 f = 159 * consistency if consistency < 2 else 255 - for name, place in self.__places.items(): + for identifier, place in self.__places.items(): try: - T = self.__places[name].translation - R = self.__places[name].rotation + T = self.__places[identifier].translation + R = self.__places[identifier].rotation # Draw place axis axisPoints = (T + numpy.float32([R.dot([l/2, 0, 0]), R.dot([0, l/2, 0]), R.dot([0, 0, l/2]), R.dot([0, 0, 0])])).reshape(-1, 3) |