Fixing .OBJ makers loading. Calculating rotation from normal vector.

1 files changed, 66 insertions, 31 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index b267102..6fe0d0f 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -94,7 +94,7 @@ class ArUcoScene():
         for name, place in self.__places.items():
 
             # Create intrinsic rotation matrix
-            R = self.__make_rotation_matrix(*place.rotation)
+            R = self.__euler_vector_to_rotation_matrix(*place.rotation)
 
             assert(self.__is_rotation_matrix(R))
 
@@ -196,20 +196,23 @@ class ArUcoScene():
         return list(self.__identifier_cache.keys())
 
     def __load_places_from_obj(self, obj_filepath: str) -> dict:
-        """Load places from .obj file."""
+        """Load places from .obj file.
+
+        .. warning:: 'o' tag string format should be DICTIONARY#IDENTIFIER_NAME
+        """
 
         self.__places = {}
 
-        # regex rules for .obj file parsing
+        # Regex rules for .obj file parsing
         OBJ_RX_DICT = {
-            'comment': re.compile(r'#(.*)\n'),
-            'name': re.compile(r'o (\w+)(.*)\n'),
+            'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'),
             'vertice': re.compile(r'v ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'),
             'normal': re.compile(r'vn ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'),
-            'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n')
+            'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n'),
+            'comment': re.compile(r'#(.*)\n') # keep comment regex after object regex because the # is used in object string too
         }
 
-        # regex .obj line parser
+        # Regex .obj line parser
         def __parse_obj_line(line):
 
             for key, rx in OBJ_RX_DICT.items():
@@ -217,72 +220,104 @@ class ArUcoScene():
                 if match:
                     return key, match
 
-            # if there are no matches
+            # If there are no matches
             return None, None
         
-        # start parsing
+        # Start parsing
         try:
 
             name = None
             vertices = []
-            normals = {}
+            markers = {}
+            rotations = {}
             faces = {}
 
-            # open the file and read through it line by line
+            # Open the file and read through it line by line
             with open(obj_filepath, 'r') as file:
 
                 line = file.readline()
 
                 while line:
 
-                    # at each line check for a match with a regex
+                    # At each line check for a match with a regex
                     key, match = __parse_obj_line(line)
 
-                    # extract comment
+                    # Extract comment
                     if key == 'comment':
                         pass
 
-                    # extract place name
-                    elif key == 'name':
+                    # Extract marker dictionary and identifier
+                    elif key == 'object':
+
+                        dictionary = str(match.group(1))
+                        identifier = int(match.group(2))
+                        last = str(match.group(3))
+
+                        # Check that marker dictionary is like the scene dictionary
+                        if dictionary == self.__dictionary.name:
 
-                        name = str(match.group(1))
+                            name = f'{dictionary}#{identifier}' # ignore last part
+                            markers[name] = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size)
 
-                    # fill vertices array
+                        else:
+
+                            raise NameError(f'Marker#{identifier} dictionary is not {self.__dictionary.name}')
+
+                    # Fill vertices array
                     elif key == 'vertice':
 
                         vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]))
 
-                    # extract normal
+                    # Extract normal to calculate rotation vectore
                     elif key == 'normal':
 
-                        normals[name] = numpy.array([float(match.group(1)), float(match.group(2)), float(match.group(3))])
+                        R = self.__normal_vector_to_rotation_matrix(float(match.group(1)), float(match.group(2)), float(match.group(3)))
+                        rvecs, _ = cv.Rodrigues(R)
+                        rvecs = rvecs.reshape(1, 3)[0]
+                        rvecs = numpy.array([numpy.rad2deg(rvecs[0]), numpy.rad2deg(rvecs[1]), numpy.rad2deg(rvecs[2])])
+
+                        print(name)
+                        print(R)
+                        print(rvecs)
+
+                        rotations[name] = rvecs
 
-                    # extract aoi3D vertice id
+                    # Extract vertice ids
                     elif key == 'face':
 
-                        faces[name] = [int(i) for i in match.group(1).split()]
+                        faces[name] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))]
 
-                    # go to next line
+                    # Go to next line
                     line = file.readline()
 
                 file.close()
 
-                # retreive all place vertices
+                # Retreive marker vertices thanks to face vertice ids
                 for name, face in faces.items():
 
-                    center = numpy.array([ vertices[i-1] for i in face ]).mean(axis=0)
-                    normal = normals[name]
+                    translation = numpy.array([ vertices[i-1] for i in face ]).mean(axis=0)
+                    rotation = rotations[name]
+                    marker = markers[name]
 
-                    # WARNING: here we set marker identifier depending on the place position in the file
-                    # TODO: extract identifiers from name
-                    marker = ArUcoMarker.ArUcoMarker(self.__dictionary, len(self.__places), self.__marker_size)
-
-                    self.__places[name] = Place(center, normal, marker) 
+                    self.__places[name] = Place(translation, rotation, marker) 
 
         except IOError:
             raise IOError(f'File not found: {obj_filepath}')
 
-    def __make_rotation_matrix(self, x, y, z):
+    def __normal_vector_to_rotation_matrix(self, x, y, z):
+        # Applying formula found at https://math.stackexchange.com/questions/1956699/getting-a-transformation-matrix-from-a-normal-vector
+
+        xy_dist = math.sqrt(x**2 + y**2)
+
+        if xy_dist > 0:
+
+            return numpy.array([[y/xy_dist, -x/xy_dist, 0], [x*z/xy_dist, y*z/xy_dist, -xy_dist], [x, y, z]])
+
+        else:
+
+            return numpy.array([[1, 0, 0], [0, 1, 0], [x, y, z]])
+
+    def __euler_vector_to_rotation_matrix(self, x, y, z):
 
         # Create rotation matrix around x axis
         c = numpy.cos(numpy.deg2rad(x))