Managing ArUcoScene places by marker identifier. Improving ArUcoScene class tests.

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)