aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoCube.py108
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)