diff options
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoCube.py | 108 |
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) |