Fixing places dictionary description loading.

1 files changed, 32 insertions, 7 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 3990904..0bdefa6 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -38,14 +38,14 @@ class Place():
 class ArUcoScene():
     """Define abstract class to handle group of ArUco markers as one unique spatial entity and estimate its pose."""
 
-    def __init__(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary, marker_size: float, places: dict | str = None):
+    def __init__(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary, marker_size: float, data_places: dict | str = None):
         """Define scene attributes."""
 
         self.__dictionary = dictionary
         self.__marker_size = marker_size
 
         # NEVER USE {} as default function argument
-        self.places = places
+        self.places = data_places
 
     @property
     def places(self) -> dict:
@@ -54,21 +54,25 @@ class ArUcoScene():
         return self.__places
     
     @places.setter
-    def places(self, places: dict | str):
+    def places(self, data: dict | str):
 
         # str: path to .obj file
-        if type(places) == str:
+        if type(data) == str:
 
-            self.__load_places_from_obj(places)
+            self.__load_places_from_obj(data)
 
         # dict: all places
         else:
   
             self.__places = {}
 
-            for name, place in places.items():
+            for name, place in data.items():
+
+                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)
-                self.__places[name] = Place(numpy.array(place['translation']).astype(numpy.float32), numpy.array(place['rotation']).astype(numpy.float32), marker)
+
+                self.__places[name] = Place(tvec, rmat, marker)
 
         # Init pose data
         self._translation = numpy.zeros(3)
@@ -169,6 +173,27 @@ class ArUcoScene():
 
         return list(self.__identifier_cache.keys())
 
+
+    def __make_rotation_matrix(self, 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))
+
     def __load_places_from_obj(self, obj_filepath: str) -> dict:
         """Load places from .obj file.