Calculating place rotation from .OBJ file. Removing translation cache and rotation cache as everything is stored into places. Using rotation matrix instead of rotation vector.

1 files changed, 60 insertions, 135 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 6fe0d0f..2ca1542 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -13,6 +13,12 @@ import numpy
 import cv2 as cv
 import cv2.aruco as aruco
 
+T0 = numpy.array([0., 0., 0.])
+"""Define no translation vector."""
+
+R0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
+"""Define no rotation matrix."""
+
 ArUcoSceneType = TypeVar('ArUcoScene', bound="ArUcoScene")
 # Type definition for type annotation convenience
 
@@ -84,29 +90,12 @@ class ArUcoScene():
         for name, place in self.__places.items():
             self.__identifier_cache[place.marker.identifier] = name
 
-        # Process each place pose to speed up further calculations
-        self.__translation_cache = {}
-        for name, place in self.__places.items():
-            self.__translation_cache[name] = place.translation
-
-        # Process each place rotation matrix to speed up further calculations
-        self.__rotation_cache = {}
-        for name, place in self.__places.items():
-
-            # Create intrinsic rotation matrix
-            R = self.__euler_vector_to_rotation_matrix(*place.rotation)
-
-            assert(self.__is_rotation_matrix(R))
-
-            # Store rotation matrix
-            self.__rotation_cache[name] = R
-
         # 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):
 
-            A = self.__rotation_cache[A_name]
-            B = self.__rotation_cache[B_name]
+            A = self.__places[A_name].rotation
+            B = self.__places[B_name].rotation
 
             if numpy.array_equal(A, B):
 
@@ -117,7 +106,7 @@ class ArUcoScene():
                 # Rotation matrix from A place to B place
                 AB = B.dot(A.T)
 
-                assert(self.__is_rotation_matrix(AB))
+                #assert(self.__is_rotation_matrix(AB))
 
                 # Calculate axis-angle representation of AB rotation matrix
                 angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
@@ -136,8 +125,8 @@ class ArUcoScene():
         self.__distance_cache = {}
         for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.__places.items(), 2):
 
-            A = self.__translation_cache[A_name]
-            B = self.__translation_cache[B_name]
+            A = self.__places[A_name].translation
+            B = self.__places[B_name].translation
 
             # Calculate axis-angle representation of AB rotation matrix
             distance = numpy.linalg.norm(B - A)
@@ -169,14 +158,6 @@ class ArUcoScene():
         for i, name in self.__identifier_cache.items():
             output += f'\n\t\t- {i}: {name}'
 
-        output += '\n\n\tTranslation cache:'
-        for name, item in self.__translation_cache.items():
-            output += f'\n\t\t- {name}: {item}'
-
-        output += '\n\n\tRotation cache:'
-        for name, item in self.__rotation_cache.items():
-            output += f'\n\t\t- {name}:\n{item}'
-
         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():
@@ -229,7 +210,7 @@ class ArUcoScene():
             name = None
             vertices = []
             markers = {}
-            rotations = {}
+            normals = {}
             faces = {}
 
             # Open the file and read through it line by line
@@ -268,19 +249,10 @@ class ArUcoScene():
 
                         vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]))
 
-                    # Extract normal to calculate rotation vectore
+                    # Extract normal to calculate rotation matrix
                     elif key == 'normal':
 
-                        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
+                        normals[name] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])
 
                     # Extract vertice ids
                     elif key == 'face':
@@ -295,87 +267,63 @@ class ArUcoScene():
                 # Retreive marker vertices thanks to face vertice ids
                 for name, face in faces.items():
 
-                    translation = numpy.array([ vertices[i-1] for i in face ]).mean(axis=0)
-                    rotation = rotations[name]
-                    marker = markers[name]
-
-                    self.__places[name] = Place(translation, rotation, marker) 
-
-        except IOError:
-            raise IOError(f'File not found: {obj_filepath}')
-
-    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)
+                    # Gather place corners from counter clockwise ordered face vertices
+                    corners = numpy.array([ vertices[i-1] for i in face ])
 
-        if xy_dist > 0:
+                    # Edit place translation as corners center
+                    T = corners.mean(axis=0)
 
-            return numpy.array([[y/xy_dist, -x/xy_dist, 0], [x*z/xy_dist, y*z/xy_dist, -xy_dist], [x, y, z]])
+                    # Edit place rotation from corners positions
+                    marker_x_axis = corners[1:3].mean(axis=0) - T
+                    marker_x_axis = marker_x_axis / numpy.linalg.norm(marker_x_axis)
 
-        else:
+                    marker_y_axis = corners[2:4].mean(axis=0) - T
+                    marker_y_axis = marker_y_axis / numpy.linalg.norm(marker_y_axis)
 
-            return numpy.array([[1, 0, 0], [0, 1, 0], [x, y, z]])
+                    marker_z_axis = normals[name]
 
-    def __euler_vector_to_rotation_matrix(self, x, y, z):
+                    # RM = P
+                    M = numpy.array([marker_x_axis, marker_y_axis, marker_z_axis])
+                    P = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
+                    R = P.dot(numpy.linalg.inv(M))
 
-        # 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]])
+                    marker = markers[name]
 
-        # 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]])
+                    self.__places[name] = Place(T, R, marker)
 
-        # 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]])
+        except IOError:
+            raise IOError(f'File not found: {obj_filepath}')
 
-        # Return intrinsic rotation matrix
-        return Rx.dot(Ry.dot(Rz))
-    
     def __is_rotation_matrix(self, R):
         """Checks if a matrix is a valid rotation matrix."""
 
         I = numpy.identity(3, dtype = R.dtype)
         return numpy.linalg.norm(I - numpy.dot(R.T, R)) < 1e-6
 
-    def __normalise_marker_pose(self, name, place, F):
-
-        # Transform place rotation into scene rotation vector
-        R = self.__rotation_cache[name]
-        rvec, _ = cv.Rodrigues(F.dot(R))
-
-        #print(f'{name} rotation vector: {rvec[0][0]:3f} {rvec[1][0]:3f} {rvec[2][0]:3f}')
-
-        # Transform place translation into scene translation vector
-        OF = place.translation
-        T = self.__translation_cache[name]
-        FC = R.dot(F.dot(-T))
+    def __normalise_marker_pose(self, place, R, T):
 
-        tvec = OF + FC
+        # Transform marker rotation into scene rotation matrix
+        Rs = place.rotation.dot(R)
 
-        #print(f'{name} translation vector: {tvec[0]:3f} {tvec[1]:3f} {tvec[2]:3f}')
+        # Transform marker translation into scene translation vector
+        Ts = T + place.rotation.dot(R.dot(-place.translation))
 
-        return rvec, tvec
+        return Rs, Ts
 
     def estimate_pose(self, tracked_markers) -> Tuple[numpy.array, numpy.array, bool, int, dict]:
         """Estimate scene pose from tracked markers (cf ArUcoTracker.track()) and validate its consistency according expected scene places.
         
         * **Returns:** 
             - scene translation vector
-            - scene rotation vector
+            - scene rotation matrix
             - scene pose estimation success status
             - all tracked markers considered as consistent and used to estimate the pose
             - dict of identified distance or angle unconsistencies and out-of-bounds values
         """
 
         # Init pose data
-        self._translation = numpy.zeros(3)
-        self._rotation = numpy.zeros(3)
+        self._translation = T0
+        self._rotation = R0
         self._succeded = False
         self._consistent_markers = {}
         self._unconsistencies = {}
@@ -396,41 +344,27 @@ class ArUcoScene():
             except KeyError:
                 continue
 
-        #print('-------------- ArUcoScene pose estimation --------------')
-
         # Pose consistency checking is'nt possible when only one marker is tracked
         if len(scene_markers) == 1:
 
-            # Get scene pose from to the unique marker
+            # Get scene pose from the unique marker
             name, marker = scene_markers.popitem()
-            F, _ = cv.Rodrigues(marker.rotation)
 
-            self._rotation, self._translation = self.__normalise_marker_pose(name, marker, F)
+            self._rotation, self._translation = self.__normalise_marker_pose(self.__places[name], marker.rotation, marker.translation)
             self._succeded = True
             self._consistent_markers[name] = marker
 
-            #print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
-            #print(f'ArUcoScene rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
-            #print(f'ArUcoScene translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
-
         # Pose consistency checking processes markers two by two
         else:
 
             consistent_markers = []
-            consistent_rvecs = []
+            consistent_rmats = []
             consistent_tvecs = []
 
             for (A_name, A_marker), (B_name, B_marker) in itertools.combinations(scene_markers.items(), 2):
 
-                #print(f'** {A_name} > {B_name}')
-
-                # Get marker rotation estimation
-                # Use rotation matrix instead of rotation vector
-                A, _ = cv.Rodrigues(A_marker.rotation)
-                B, _ = cv.Rodrigues(B_marker.rotation)
-
                 # Rotation matrix from A marker to B marker
-                AB = B.dot(A.T)
+                AB = B_marker.rotation.dot(A_marker.rotation.T)
 
                 assert(self.__is_rotation_matrix(AB))
 
@@ -453,10 +387,10 @@ class ArUcoScene():
                         # Remember this marker is already validated
                         consistent_markers.append(A_name)
 
-                        rvec, tvec = self.__normalise_marker_pose(A_name, A_marker, A)
+                        rmat, tvec = self.__normalise_marker_pose(self.__places[A_name], A_marker.rotation, A_marker.translation)
 
                         # Store normalised marker pose
-                        consistent_rvecs.append(rvec)
+                        consistent_rmats.append(rmat)
                         consistent_tvecs.append(tvec)
                         
                     if B_name not in consistent_markers:
@@ -464,10 +398,10 @@ class ArUcoScene():
                         # Remember this marker is already validated
                         consistent_markers.append(B_name)
 
-                        rvec, tvec = self.__normalise_marker_pose(B_name, B_marker, B)
+                        rmat, tvec = self.__normalise_marker_pose(self.__places[B_name], B_marker.rotation, B_marker.translation)
 
                         # Store normalised marker pose
-                        consistent_rvecs.append(rvec)
+                        consistent_rmats.append(rmat)
                         consistent_tvecs.append(tvec)
 
                 else:
@@ -482,21 +416,17 @@ class ArUcoScene():
 
                 # Consider ArUcoScene rotation as the mean of all consistent translations
                 # !!! 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(consistent_rvecs), axis=0)
+                self._rotation = numpy.mean(numpy.array(consistent_rmats), axis=0)
 
                 # Consider ArUcoScene translation as the mean of all consistent translations
                 self._translation = numpy.mean(numpy.array(consistent_tvecs), axis=0)
 
-                #print(':::::::::::::::::::::::::::::::::::::::::::::::::::')
-                #print(f'ArUcoScene rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
-                #print(f'ArUcoScene translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
-
                 self._succeded = True
                 self._consistent_markers = consistent_markers
 
             else:
 
-                unconsistent_rvecs = []
+                unconsistent_rmats = []
                 unconsistent_tvecs = []
 
                 # Gather unconsistent pose estimations
@@ -504,23 +434,18 @@ class ArUcoScene():
 
                     if name not in consistent_markers:
 
-                        R, _ = cv.Rodrigues(marker.rotation)
-                        rvec, tvec = self.__normalise_marker_pose(name, marker, R)
+                        rmat, tvec = self.__normalise_marker_pose(self.__places[name], marker.rotation, marker.translation)
 
-                        unconsistent_rvecs = [rvec]
+                        unconsistent_rmats = [rmat]
                         unconsistent_tvecs = [tvec]
 
                 # Consider ArUcoScene rotation as the mean of all unconsistent translations
                 # !!! 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(unconsistent_rvecs), axis=0)
+                self._rotation = numpy.mean(numpy.array(unconsistent_rmats), axis=0)
 
                 # Consider ArUcoScene translation as the mean of all unconsistent translations
                 self._translation = numpy.mean(numpy.array(unconsistent_tvecs), axis=0)
 
-                #print(':::::::::::::::::::::::::::::::::::::::::::::::::::')
-                #print(f'ArUcoScene rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
-                #print(f'ArUcoScene translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
-
                 self._succeded = False
                 self._consistent_markers = {}
 
@@ -546,17 +471,17 @@ class ArUcoScene():
 
     @property
     def rotation(self) -> numpy.array:
-        """Access to scene rotation vector.
+        """Access to scene rotation matrix.
 
         .. warning::
-           Setting scene rotation vector implies succeded status to be True and consistency score to be 0."""
+           Setting scene rotation matrix implies succeded status to be True and consistency score to be 0."""
 
         return self._translation
 
     @rotation.setter
-    def rotation(self, rvec):
+    def rotation(self, rmat):
 
-        self._rotation = rvec
+        self._rotation = rmat
         self._succeded = True
         self._consistent_markers = 0
 
@@ -601,8 +526,8 @@ class ArUcoScene():
                     if name != "top":
                         continue
 
-                    T = self.__translation_cache[name]
-                    R = self.__rotation_cache[name]
+                    T = self.__places[name].translation
+                    R = self.__places[name].rotation
 
                     placePoints = (T + numpy.float32([R.dot([-l, -l, 0]), R.dot([l, -l, 0]), R.dot([l, l, 0]), R.dot([-l, l, 0])])).reshape(-1, 3)
                     placePoints, _ = cv.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))