Improving ArUcoScene loading.

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()).