aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze.test/ArUcoMarkers/ArUcoScene.py59
-rw-r--r--src/argaze.test/ArUcoMarkers/utils/scene.json18
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py343
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()).