diff options
| -rw-r--r-- | src/argaze.test/ArUcoMarkers/ArUcoScene.py | 139 | ||||
| -rw-r--r-- | src/argaze.test/ArUcoMarkers/utils/scene.obj | 25 | ||||
| -rw-r--r-- | src/argaze.test/utils/environment.json | 10 | ||||
| -rw-r--r-- | src/argaze/ArFeatures.py | 36 | ||||
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 297 |
5 files changed, 333 insertions, 174 deletions
diff --git a/src/argaze.test/ArUcoMarkers/ArUcoScene.py b/src/argaze.test/ArUcoMarkers/ArUcoScene.py index 537469e..24e5347 100644 --- a/src/argaze.test/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze.test/ArUcoMarkers/ArUcoScene.py @@ -4,34 +4,151 @@ import unittest import os import math -from argaze.ArUcoMarkers import ArUcoScene +from argaze.ArUcoMarkers import ArUcoScene, ArUcoMarker import cv2 as cv import numpy class TestArUcoSceneClass(unittest.TestCase): - """Test ArUcoScene class.""" - def test_new(self): - """Test ArUcoScene creation.""" + def setUp(self): + """Initialize ArUcoScene class test.""" # Edit file path current_directory = os.path.dirname(os.path.abspath(__file__)) obj_filepath = os.path.join(current_directory, 'utils/scene.obj') # Load file - aruco_scene = ArUcoScene.ArUcoScene(1, obj_filepath) + self.aruco_scene = ArUcoScene.ArUcoScene(obj_filepath) + + # Prepare detected markers + self.detected_markers = { + 0: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 0, 1.), + 1: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 1, 1.), + 2: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 2, 1.), + 3: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 3, 1.) + } + + # Prepare scene markers and remaining markers + self.scene_markers, self.remaining_markers = self.aruco_scene.filter_markers(self.detected_markers) + + def test_new(self): + """Test ArUcoScene creation.""" # Check ArUcoScene creation - self.assertEqual(len(aruco_scene.places), 2) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.identifiers, [0, 1])) + self.assertEqual(len(self.aruco_scene.places), 3) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.identifiers, [0, 1, 2])) + self.assertEqual(self.aruco_scene.marker_size, 1.) + + self.assertEqual(self.aruco_scene.places[0].marker.identifier, 0) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].translation, [0., 0., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_scene.places[1].marker.identifier, 1) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].translation, [10., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_scene.places[2].marker.identifier, 2) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].translation, [0., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + def test_filter_markers(self): + """Test ArUcoScene markers filtering.""" + + # Check scene markers and remaining markers + self.assertEqual(len(self.scene_markers), 3) + self.assertEqual(len(self.remaining_markers), 1) + + self.assertIsNone(numpy.testing.assert_array_equal(list(self.scene_markers.keys()), self.aruco_scene.identifiers)) + self.assertIsNone(numpy.testing.assert_array_equal(list(self.remaining_markers.keys()), [3])) + + def test_check_markers_consistency(self): + """Test ArUcoScene markers consistency checking.""" + + # Edit consistent marker poses + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[1].translation = numpy.array([11., 11., 5.]) + self.scene_markers[1].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[2].translation = numpy.array([1., 11., 5.]) + self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + # Check consistency + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.check_markers_consistency(self.scene_markers, 1, 1) + + # Check consistent markers, unconsistent markers and unconsistencies + self.assertEqual(len(consistent_markers), 3) + self.assertEqual(len(unconsistent_markers), 0) + self.assertEqual(len(unconsistencies), 0) + + self.assertIsNone(numpy.testing.assert_array_equal(list(consistent_markers.keys()), self.aruco_scene.identifiers)) + + # Edit unconsistent marker poses + self.scene_markers[2].translation = numpy.array([5., 15., 5.]) + + # Check consistency + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.check_markers_consistency(self.scene_markers, 1, 1) + + # Check consistent markers, unconsistent markers and unconsistencies + self.assertEqual(len(consistent_markers), 2) + self.assertEqual(len(unconsistent_markers), 1) + self.assertEqual(len(unconsistencies), 2) + + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistent_markers.keys()), [2])) + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistencies.keys()), ['0/2 distance', '1/2 distance'])) + + def test_estimate_pose_from_single_marker(self): + """Test ArUcoScene pose estimation from single marker.""" + + # Edit marke pose + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + # Estimate pose + tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(self.scene_markers[0]) + + self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) + self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + def test_estimate_pose_from_markers(self): + """Test ArUcoScene pose estimation from markers.""" + + # Edit markers pose + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[1].translation = numpy.array([11., 11., 5.]) + self.scene_markers[1].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[2].translation = numpy.array([1., 11., 5.]) + self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + # Estimate pose + tvec, rmat = self.aruco_scene.estimate_pose_from_markers(self.scene_markers) + + self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) + self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + def test_estimate_pose_from_axis_markers(self): + """Test ArUcoScene pose estimation from axis markers.""" + + # Edit markers pose + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[1].translation = numpy.array([11., 11., 5.]) + self.scene_markers[1].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#0'].translation, [0.5, 0.5, 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#0'].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + self.scene_markers[2].translation = numpy.array([1., 11., 5.]) + self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#1'].translation, [10.5, 10.5, 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#1'].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + # Estimate pose + tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(self.scene_markers[2], self.scene_markers[1], self.scene_markers[0]) + self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) + self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]])) if __name__ == '__main__': diff --git a/src/argaze.test/ArUcoMarkers/utils/scene.obj b/src/argaze.test/ArUcoMarkers/utils/scene.obj index 51c8148..16c22a0 100644 --- a/src/argaze.test/ArUcoMarkers/utils/scene.obj +++ b/src/argaze.test/ArUcoMarkers/utils/scene.obj @@ -1,17 +1,22 @@ # .OBJ file for ArUcoScene unitary test o DICT_ARUCO_ORIGINAL#0_Marker -v 0.000000 0.000000 0.000000 -v 1.000000 0.000000 0.000000 -v 0.000000 1.000000 0.000000 -v 1.000000 1.000000 0.000000 +v -0.500000 -0.500000 0.000000 +v 0.500000 -0.500000 0.000000 +v -0.500000 0.500000 0.000000 +v 0.500000 0.500000 0.000000 vn 0.0000 0.0000 1.0000 -s off f 1//1 2//1 4//1 3//1 o DICT_ARUCO_ORIGINAL#1_Marker -v 10.000000 10.000000 0.000000 -v 11.000000 10.000000 0.000000 -v 10.000000 11.000000 0.000000 -v 11.000000 11.000000 0.000000 +v 9.500000 9.500000 0.000000 +v 10.500000 9.500000 0.000000 +v 9.500000 10.500000 0.000000 +v 10.500000 10.500000 0.000000 vn 0.0000 0.0000 1.0000 -s off f 5//2 6//2 8//2 7//2 +o DICT_ARUCO_ORIGINAL#2_Marker +v -0.500000 9.500000 0.000000 +v 0.500000 9.500000 0.000000 +v -0.500000 10.500000 0.000000 +v 0.500000 10.500000 0.000000 +vn 0.0000 0.0000 1.0000 +f 9//3 10//3 12//3 11//3 diff --git a/src/argaze.test/utils/environment.json b/src/argaze.test/utils/environment.json index d5f8639..e4cf43a 100644 --- a/src/argaze.test/utils/environment.json +++ b/src/argaze.test/utils/environment.json @@ -42,15 +42,13 @@ }, "TestScene" : { "aruco_scene": { - "A": { + "0": { "translation": [1, 0, 0], - "rotation": [0, 0, 0], - "marker": 0 + "rotation": [0, 0, 0] }, - "B": { + "1": { "translation": [0, 1, 0], - "rotation": [0, 90, 0], - "marker": 1 + "rotation": [0, 90, 0] } }, "aoi_scene": "aoi.obj", diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py index e9b7974..4e1f5cb 100644 --- a/src/argaze/ArFeatures.py +++ b/src/argaze/ArFeatures.py @@ -122,7 +122,7 @@ class ArScene(): aruco_scene_value = os.path.join(self.__ar_environment.working_directory, aruco_scene_value) - self.aruco_scene = ArUcoScene.ArUcoScene(self.__ar_environment.aruco_detector.marker_size, aruco_scene_value, self.__ar_environment.aruco_detector.dictionary) + self.aruco_scene = ArUcoScene.ArUcoScene(aruco_scene_value, self.__ar_environment.aruco_detector.dictionary, self.__ar_environment.aruco_detector.marker_size) # Check aoi_scene value type aoi_scene_value = kwargs.pop('aoi_scene') @@ -146,19 +146,6 @@ class ArScene(): # Update all attributes from arguments self.__dict__.update(kwargs) - # Convert aruco axis markers identifier into ARUCO_DICT_NAME#ID string - aruco_axis_string = {} - for axis_name, markers_id in self.aruco_axis.items(): - - # Estimate pose from axis markers - aruco_axis_names = [] - for marker_id in markers_id: - aruco_axis_names.append(f'{ar_environment.aruco_detector.dictionary.name}#{marker_id}') - - aruco_axis_string[axis_name] = aruco_axis_names - - self.aruco_axis = aruco_axis_string - # Preprocess orthogonal projection to speed up further aruco aoi processings self.__orthogonal_projection_cache = self.orthogonal_projection @@ -217,24 +204,25 @@ class ArScene(): # Estimate scene pose from unique marker transformations elif len(scene_markers) == 1: - tvec, rmat = self.aruco_scene.estimate_pose_from_any_markers(scene_markers) + marker_id, marker = scene_markers.popitem() + tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(marker) - return tvec, rmat, scene_markers + return tvec, rmat, {marker_id: marker} # Try to estimate scene pose from 3 markers defining an orthogonal axis elif len(scene_markers) >= 3 and len(self.aruco_axis) > 0: - for axis_name, markers_names in self.aruco_axis.items(): + for axis_name, axis_markers in self.aruco_axis.items(): try: - axis_markers = [] - for name in markers_names: - axis_markers.append((name, scene_markers[name])) + origin_marker = scene_markers[axis_markers['origin_marker']] + horizontal_axis_marker = scene_markers[axis_markers['horizontal_axis_marker']] + vertical_axis_marker = scene_markers[axis_markers['vertical_axis_marker']] - tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(axis_markers) + tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker) - return tvec, rmat, axis_markers + return tvec, rmat, {origin_marker.identifier: origin_marker, horizontal_axis_marker.identifier: horizontal_axis_marker, vertical_axis_marker.identifier: vertical_axis_marker} except: pass @@ -249,8 +237,8 @@ class ArScene(): raise PoseEstimationFailed('Unconsistent marker poses', unconsistencies) - # Otherwise, estimate scene pose from all markers transformations - tvec, rmat = self.aruco_scene.estimate_pose_from_any_markers(consistent_markers) + # Otherwise, estimate scene pose from all consistent markers pose + tvec, rmat = self.aruco_scene.estimate_pose_from_markers(consistent_markers) return tvec, rmat, consistent_markers 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) |