Factorizing code. Setting rotation and translation as internal data.

1 files changed, 68 insertions, 40 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoCube.py b/src/argaze/ArUcoMarkers/ArUcoCube.py
index 552d4d1..a1090fe 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCube.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCube.py
@@ -40,12 +40,6 @@ class ArUcoCube():
     faces: dict = field(init=False, default_factory=dict)
     """All named faces of the cube and their ArUco markers."""
 
-    translation: numpy.array = field(init=False)
-    """Position of the cube."""
-
-    rotation: numpy.array = field(init=False)
-    """Rotation of the cube."""
-
     angle_tolerance: float = field(init=False)
     """Angle error tolerance allowed to validate face pose in degree."""
 
@@ -83,8 +77,10 @@ class ArUcoCube():
             self.distance_tolerance = configuration['distance_tolerance']
 
             # Init pose data
-            self.translation = numpy.zeros(3)
-            self.rotation = numpy.zeros(3)
+            self.__translation = numpy.zeros(3)
+            self.__rotation = numpy.zeros(3)
+            self.__succeded = False
+            self.__validated = False
 
             # Process markers ids to speed up further calculations
             self.__identifier_cache = {}
@@ -100,23 +96,8 @@ class ArUcoCube():
             self.__rotation_cache = {}
             for name, face in self.faces.items():
 
-                # Create rotation matrix around x axis
-                c = numpy.cos(numpy.deg2rad(face.rotation[0]))
-                s = numpy.sin(numpy.deg2rad(face.rotation[0]))
-                Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]])
-
-                # Create rotation matrix around y axis
-                c = numpy.cos(numpy.deg2rad(face.rotation[1]))
-                s = numpy.sin(numpy.deg2rad(face.rotation[1]))
-                Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
-
-                # Create rotation matrix around z axis
-                c = numpy.cos(numpy.deg2rad(face.rotation[2]))
-                s = numpy.sin(numpy.deg2rad(face.rotation[2]))
-                Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
-
                 # Create intrinsic rotation matrix
-                R = Rx.dot(Ry.dot(Rz))
+                R = self.__make_rotation_matrix(*face.rotation)
 
                 assert(self.__is_rotation_matrix(R))
 
@@ -172,6 +153,26 @@ class ArUcoCube():
                 print(f'- {A_name}/{B_name}: {angle:3f}')
 
         print(f'\nDistance cache: {self.__distance_cache}')
+
+    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 __is_rotation_matrix(self, R):
         """Checks if a matrix is a valid rotation matrix."""
@@ -200,6 +201,12 @@ class ArUcoCube():
 
     def estimate_pose(self, tracked_markers):
 
+        # Init pose data
+        self.__translation = numpy.zeros(3)
+        self.__rotation = numpy.zeros(3)
+        self.__succeded = False
+        self.__validated = False
+
         # Look for faces related to tracked markers
         tracked_faces = {}
         for (marker_id, marker) in tracked_markers.items():
@@ -220,13 +227,12 @@ class ArUcoCube():
             name, face = tracked_faces.popitem()
             F, _ = cv.Rodrigues(face.rotation)
 
-            self.rotation, self.translation = self.__normalise_face_pose(name,face, F)
+            self.__rotation, self.__translation = self.__normalise_face_pose(name,face, F)
+            self.__succeded = True
 
             #print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
-            #print(f'arcube rotation vector: {self.rotation[0][0]:3f} {self.rotation[1][0]:3f} {self.rotation[2][0]:3f}')
-            #print(f'arcube translation vector: {self.translation[0]:3f} {self.translation[1]:3f} {self.translation[2]:3f}')
-
-            return False
+            #print(f'arcube rotation vector: {self.__rotation[0][0]:3f} {self.__rotation[1][0]:3f} {self.__rotation[2][0]:3f}')
+            #print(f'arcube translation vector: {self.__translation[0]:3f} {self.__translation[1]:3f} {self.__translation[2]:3f}')
 
         # Pose validity checking processes faces two by two
         else:
@@ -296,21 +302,43 @@ class ArUcoCube():
 
                 # Consider arcube rotation as the mean of all valid 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(valid_rvecs), axis=0)
+                self.__rotation = numpy.mean(numpy.array(valid_rvecs), axis=0)
 
                 # Consider arcube translation as the mean of all valid translations
-                self.translation = numpy.mean(numpy.array(valid_tvecs), axis=0)
+                self.__translation = numpy.mean(numpy.array(valid_tvecs), axis=0)
 
                 #print(':::::::::::::::::::::::::::::::::::::::::::::::::::')
-                #print(f'arcube rotation vector: {self.rotation[0][0]:3f} {self.rotation[1][0]:3f} {self.rotation[2][0]:3f}')
-                #print(f'arcube translation vector: {self.translation[0]:3f} {self.translation[1]:3f} {self.translation[2]:3f}')
-
-                return True
+                #print(f'arcube rotation vector: {self.__rotation[0][0]:3f} {self.__rotation[1][0]:3f} {self.__rotation[2][0]:3f}')
+                #print(f'arcube translation vector: {self.__translation[0]:3f} {self.__translation[1]:3f} {self.__translation[2]:3f}')
 
-        raise ValueError('Cube pose can\'t be estimated.')
+                self.__succeded = True
+                self.__validated = True
 
         #print('----------------------------------------------------')
 
+        return self.get_pose()
+
+    def get_pose(self):
+
+        return self.__translation, self.__rotation, self.__succeded, self.__validated
+
+    def offset_pose(self, tvec = numpy.zeros(3), rvec = numpy.zeros(3)):
+
+        # Offset cube translation
+        self.__translation += tvec
+
+        # Offset cube rotation
+        C, _ = cv.Rodrigues(self.__rotation)
+
+        R_offset = self.__make_rotation_matrix(*rvec)
+        C_offset = C.dot(R_offset.T)  
+
+        self.__rotation, _ = cv.Rodrigues(C_offset)
+
+        # Pose is no more validated
+        self.__succeded = True
+        self.__validated = False
+
     def draw(self, frame, K, D):
 
         l = self.edge_size / 2
@@ -318,7 +346,7 @@ class ArUcoCube():
 
         # Draw axis
         axisPoints = numpy.float32([[ll, 0, 0], [0, ll, 0], [0, 0, ll], [0, 0, 0]]).reshape(-1, 3)
-        axisPoints, _ = cv.projectPoints(axisPoints, self.rotation, self.translation, K, D)
+        axisPoints, _ = cv.projectPoints(axisPoints, self.__rotation, self.__translation, K, D)
         axisPoints = axisPoints.astype(int)
 
         frame = cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0,0,255), 5) # X (red)
@@ -327,7 +355,7 @@ class ArUcoCube():
 
         # Draw left face
         leftPoints = numpy.float32([[-l, l, l], [-l, -l, l], [-l, -l, -l], [-l, l, -l]]).reshape(-1, 3)
-        leftPoints, _ = cv.projectPoints(leftPoints, self.rotation, self.translation, K, (0, 0, 0, 0))
+        leftPoints, _ = cv.projectPoints(leftPoints, self.__rotation, self.__translation, K, (0, 0, 0, 0))
         leftPoints = leftPoints.astype(int)
         
         frame = cv.line(frame, tuple(leftPoints[0].ravel()), tuple(leftPoints[1].ravel()), (0,0,255), 2)
@@ -337,7 +365,7 @@ class ArUcoCube():
 
         # Draw top face
         topPoints = numpy.float32([[l, l, l], [-l, l, l], [-l, l, -l], [l, l, -l]]).reshape(-1, 3)
-        topPoints, _ = cv.projectPoints(topPoints, self.rotation, self.translation, K, (0, 0, 0, 0))
+        topPoints, _ = cv.projectPoints(topPoints, self.__rotation, self.__translation, K, (0, 0, 0, 0))
         topPoints = topPoints.astype(int)
 
         frame = cv.line(frame, tuple(topPoints[0].ravel()), tuple(topPoints[1].ravel()), (0,255,0), 2)
@@ -347,7 +375,7 @@ class ArUcoCube():
 
         # Draw front face
         frontPoints = numpy.float32([[l, l, l], [-l, l, l], [-l, -l, l], [l, -l, l]]).reshape(-1, 3)
-        frontPoints, _ = cv.projectPoints(frontPoints, self.rotation, self.translation, K, (0, 0, 0, 0))
+        frontPoints, _ = cv.projectPoints(frontPoints, self.__rotation, self.__translation, K, (0, 0, 0, 0))
         frontPoints = frontPoints.astype(int)
         
         frame = cv.line(frame, tuple(frontPoints[0].ravel()), tuple(frontPoints[1].ravel()), (255,0,0), 2)