diff options
| -rw-r--r-- | src/argaze.test/ArUcoMarkers/ArUcoScene.py | 59 | ||||
| -rw-r--r-- | src/argaze.test/ArUcoMarkers/utils/scene.json | 18 | ||||
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 343 |
3 files changed, 253 insertions, 167 deletions
diff --git a/src/argaze.test/ArUcoMarkers/ArUcoScene.py b/src/argaze.test/ArUcoMarkers/ArUcoScene.py index 24e5347..8d344dc 100644 --- a/src/argaze.test/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze.test/ArUcoMarkers/ArUcoScene.py @@ -11,15 +11,25 @@ import numpy class TestArUcoSceneClass(unittest.TestCase): - def setUp(self): - """Initialize ArUcoScene class test.""" + def new_from_obj(self): # Edit file path current_directory = os.path.dirname(os.path.abspath(__file__)) obj_filepath = os.path.join(current_directory, 'utils/scene.obj') # Load file - self.aruco_scene = ArUcoScene.ArUcoScene(obj_filepath) + self.aruco_scene = ArUcoScene.ArUcoScene.from_obj(obj_filepath) + + def new_from_json(self): + + # Edit file path + current_directory = os.path.dirname(os.path.abspath(__file__)) + json_filepath = os.path.join(current_directory, 'utils/scene.json') + + # Load file + self.aruco_scene = ArUcoScene.ArUcoScene.from_json(json_filepath) + + def setup_markers(self): # Prepare detected markers self.detected_markers = { @@ -32,9 +42,35 @@ class TestArUcoSceneClass(unittest.TestCase): # Prepare scene markers and remaining markers self.scene_markers, self.remaining_markers = self.aruco_scene.filter_markers(self.detected_markers) - def test_new(self): + def test_new_from_obj(self): """Test ArUcoScene creation.""" + self.new_from_obj() + self.setup_markers() + + # Check ArUcoScene creation + 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_new_from_json(self): + """Test ArUcoScene creation.""" + + self.new_from_json() + self.setup_markers() + # Check ArUcoScene creation self.assertEqual(len(self.aruco_scene.places), 3) self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.identifiers, [0, 1, 2])) @@ -55,6 +91,9 @@ class TestArUcoSceneClass(unittest.TestCase): def test_filter_markers(self): """Test ArUcoScene markers filtering.""" + self.new_from_obj() + self.setup_markers() + # Check scene markers and remaining markers self.assertEqual(len(self.scene_markers), 3) self.assertEqual(len(self.remaining_markers), 1) @@ -65,6 +104,9 @@ class TestArUcoSceneClass(unittest.TestCase): def test_check_markers_consistency(self): """Test ArUcoScene markers consistency checking.""" + self.new_from_obj() + self.setup_markers() + # 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.]]) @@ -102,6 +144,9 @@ class TestArUcoSceneClass(unittest.TestCase): def test_estimate_pose_from_single_marker(self): """Test ArUcoScene pose estimation from single marker.""" + self.new_from_obj() + self.setup_markers() + # 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.]]) @@ -115,6 +160,9 @@ class TestArUcoSceneClass(unittest.TestCase): def test_estimate_pose_from_markers(self): """Test ArUcoScene pose estimation from markers.""" + self.new_from_obj() + self.setup_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.]]) @@ -134,6 +182,9 @@ class TestArUcoSceneClass(unittest.TestCase): def test_estimate_pose_from_axis_markers(self): """Test ArUcoScene pose estimation from axis markers.""" + self.new_from_obj() + self.setup_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.]]) diff --git a/src/argaze.test/ArUcoMarkers/utils/scene.json b/src/argaze.test/ArUcoMarkers/utils/scene.json new file mode 100644 index 0000000..3a33b53 --- /dev/null +++ b/src/argaze.test/ArUcoMarkers/utils/scene.json @@ -0,0 +1,18 @@ +{ + "dictionary": "DICT_ARUCO_ORIGINAL", + "marker_size": 1, + "places": { + "0": { + "translation": [0, 0, 0], + "rotation": [0, 0, 0] + }, + "1": { + "translation": [10, 10, 0], + "rotation": [0, 0, 0] + }, + "2": { + "translation": [0, 10, 0], + "rotation": [0, 0, 0] + } + } +} diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index 15e7a35..bc89287 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, places: dict | str, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None, marker_size: float = 0.): + def __init__(self, places: dict, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None, marker_size: float = 0.): """Define scene attributes.""" # Init expected marker size @@ -61,164 +61,25 @@ class ArUcoScene(): # NEVER USE {} as default function argument 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 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: - - self.__load_places_from_obj(data) - - # dict: all places - else: - - self.__places = {} - - 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, identifier, self.__marker_size) - - self.__places[identifier] = Place(tvec, rmat, marker) - - # Init pose data - self._translation = numpy.zeros(3) - self._rotation = numpy.zeros(3) - - # Process axis-angle between place combination to speed up further calculations - self.__angle_cache = {} - for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2): - - A = self.__places[A_identifier].rotation - B = self.__places[B_identifier].rotation - - if numpy.array_equal(A, B): - - angle = 0. - - else: - - # Rotation matrix from A place to B place - AB = B.dot(A.T) - - # Calculate axis-angle representation of AB rotation matrix - angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) - - try: - self.__angle_cache[A_identifier][B_identifier] = angle - except: - self.__angle_cache[A_identifier] = {B_identifier: angle} - - try: - self.__angle_cache[B_identifier][A_identifier] = angle - except: - self.__angle_cache[B_identifier] = {A_identifier: angle} - - # Process distance between each place combination to speed up further calculations - self.__distance_cache = {} - for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2): - - 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_identifier][B_identifier] = distance - except: - self.__distance_cache[A_identifier] = {B_identifier: distance} - - try: - self.__distance_cache[B_identifier][A_identifier] = distance - except: - self.__distance_cache[B_identifier] = {A_identifier: distance} - @classmethod - def from_json(self, json_filepath) -> ArUcoSceneType: - """Load ArUco scene from .json file.""" - - with open(json_filepath) as configuration_file: - - return ArUcoScene(**json.load(configuration_file)) - - def __str__(self) -> str: - """String display""" - - output = f'\n\n\tDictionary: {self.__dictionary.name}' + def from_obj(self, obj_filepath: str) -> ArUcoSceneType: + """Load ArUco scene from .obj file. - output += '\n\n\tPlaces:' - 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\tAngle cache:' - 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_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}' + .. note:: + Expected object (o) name format: <DICTIONARY>#<IDENTIFIER>_Marker - return output - - @property - def identifiers(self) -> list: - """List place maker identifiers belonging to the scene.""" - - return list(self.__places.keys()) - - def __make_rotation_matrix(self, x, y, z): + .. note:: + All markers have to belong to the same dictionary. - # Create rotation matrix around x axis - c = numpy.cos(numpy.deg2rad(x)) - s = numpy.sin(numpy.deg2rad(x)) - Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) + .. note:: + Marker normal vectors (vn) expected. - # Create rotation matrix around y axis - c = numpy.cos(numpy.deg2rad(y)) - s = numpy.sin(numpy.deg2rad(y)) - Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) - - # Create rotation matrix around z axis - c = numpy.cos(numpy.deg2rad(z)) - s = numpy.sin(numpy.deg2rad(z)) - Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) - - # Return intrinsic rotation matrix - return Rx.dot(Ry.dot(Rz)) - - def __load_places_from_obj(self, obj_filepath: str) -> dict: - - self.__places = {} - self.__marker_size = 0 + """ + new_places = {} + new_dictionary = None + new_marker_size = 0 + # Regex rules for .obj file parsing OBJ_RX_DICT = { 'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'), @@ -244,7 +105,6 @@ class ArUcoScene(): identifier = None vertices = [] - markers = {} normals = {} faces = {} @@ -269,14 +129,15 @@ class ArUcoScene(): identifier = int(match.group(2)) last = str(match.group(3)) - # Check that marker dictionary is like the scene dictionary - if dictionary == self.__dictionary.name: + # Init new scene dictionary with first dictionary name + if new_dictionary == None: - markers[identifier] = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size) + new_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(dictionary) - else: + # Check all others marker dictionary are equal to new scene dictionary + elif dictionary != new_dictionary.name: - raise NameError(f'Marker {identifier} dictionary is not {self.__dictionary.name}') + raise NameError(f'Marker {identifier} dictionary is not {new_dictionary.name}') # Fill vertices array elif key == 'vertice': @@ -329,19 +190,175 @@ class ArUcoScene(): current_marker_size = place_x_axis_norm*2 # Check that all markers size are almost equal - if self.__marker_size > 0: + if new_marker_size > 0: - if not math.isclose(current_marker_size, self.__marker_size, rel_tol=1e-3): + if not math.isclose(current_marker_size, new_marker_size, rel_tol=1e-3): raise ValueError('Markers size should be almost equal.') - self.__marker_size = current_marker_size + new_marker_size = current_marker_size - self.__places[identifier] = Place(Tp, Rp, markers[identifier]) + # Create a new place related to a new marker + new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) + new_places[identifier] = Place(Tp, Rp, new_marker) except IOError: raise IOError(f'File not found: {obj_filepath}') + return ArUcoScene(new_places, new_dictionary, new_marker_size) + + @classmethod + def from_json(self, json_filepath: str) -> ArUcoSceneType: + """Load ArUco scene from .json file.""" + + new_places = {} + new_dictionary = None + new_marker_size = 0 + + def __make_rotation_matrix(x, y, z): + + # Create rotation matrix around x axis + c = numpy.cos(numpy.deg2rad(x)) + s = numpy.sin(numpy.deg2rad(x)) + Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) + + # Create rotation matrix around y axis + c = numpy.cos(numpy.deg2rad(y)) + s = numpy.sin(numpy.deg2rad(y)) + Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) + + # Create rotation matrix around z axis + c = numpy.cos(numpy.deg2rad(z)) + s = numpy.sin(numpy.deg2rad(z)) + Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + + # Return intrinsic rotation matrix + return Rx.dot(Ry.dot(Rz)) + + with open(json_filepath) as configuration_file: + + data = json.load(configuration_file) + + new_dictionary = data.pop('dictionary') + new_marker_size = data.pop('marker_size') + + for identifier, place in data.pop('places').items(): + + # Convert string identifier to int value + identifier = int(identifier) + + tvec = numpy.array(place.pop('translation')).astype(numpy.float32) + rmat = __make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32) + + new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) + + new_places[identifier] = Place(tvec, rmat, new_marker) + + return ArUcoScene(new_places, new_dictionary, new_marker_size) + + @property + def marker_size(self) -> float: + """Expected size of all markers in the scene.""" + + return self.__marker_size + + @property + def places(self) -> dict: + """All places of the scene and their related ArUco markers.""" + + return self.__places + + @places.setter + def places(self, places: dict): + """Initialize cached data to speed up further processings.""" + + # Init places + self.__places = places + + # Init pose data + self._translation = numpy.zeros(3) + self._rotation = numpy.zeros(3) + + # Process axis-angle between place combination to speed up further calculations + self.__angle_cache = {} + for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2): + + A = self.__places[A_identifier].rotation + B = self.__places[B_identifier].rotation + + if numpy.array_equal(A, B): + + angle = 0. + + else: + + # Rotation matrix from A place to B place + AB = B.dot(A.T) + + # Calculate axis-angle representation of AB rotation matrix + angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) + + try: + self.__angle_cache[A_identifier][B_identifier] = angle + except: + self.__angle_cache[A_identifier] = {B_identifier: angle} + + try: + self.__angle_cache[B_identifier][A_identifier] = angle + except: + self.__angle_cache[B_identifier] = {A_identifier: angle} + + # Process distance between each place combination to speed up further calculations + self.__distance_cache = {} + for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2): + + 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_identifier][B_identifier] = distance + except: + self.__distance_cache[A_identifier] = {B_identifier: distance} + + try: + self.__distance_cache[B_identifier][A_identifier] = distance + except: + self.__distance_cache[B_identifier] = {A_identifier: distance} + + def __str__(self) -> str: + """String display""" + + output = f'\n\n\tDictionary: {self.__dictionary.name}' + + output = f'\n\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.translation}' + output += f'\n{place.rotation}' + + output += '\n\n\tAngle cache:' + 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_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 place maker identifiers belonging to the scene.""" + + return list(self.__places.keys()) + 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()). |