diff options
Diffstat (limited to 'src/argaze/ArUcoMarkers/ArUcoCube.py')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoCube.py | 90 |
1 files changed, 60 insertions, 30 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoCube.py b/src/argaze/ArUcoMarkers/ArUcoCube.py index 51ea314..3d889da 100644 --- a/src/argaze/ArUcoMarkers/ArUcoCube.py +++ b/src/argaze/ArUcoMarkers/ArUcoCube.py @@ -1,5 +1,6 @@ #!/usr/bin/env python +from typing import Tuple from dataclasses import dataclass, field import json import math @@ -201,7 +202,13 @@ class ArUcoCube(): def estimate_pose(self, tracked_markers): """Estimate cube pose from tracked markers (cf ArUcoTracker.track()) - **Returns:** numpy.array, numpy.array, bool, int""" + + * **Returns:** + - translation vector + - rotation vector + - pose estimation success status + - the number of faces used to estimate the pose as validity score + """ # Init pose data self.__translation = numpy.zeros(3) @@ -326,27 +333,52 @@ class ArUcoCube(): return self.get_pose() - def get_pose(self): - """Get cube pose. - **Returns:** numpy.array, numpy.array, bool, int""" + @property + def translation(self) -> numpy.array: + """Access to cube translation vector. - return self.__translation, self.__rotation, self.__succeded, self.__validity + .. warning:: + Setting cube translation vector implies succeded status to be True and validity score to be 0.""" - def set_pose(self, tvec = numpy.array([]), rvec = numpy.array([])): - """Set cube pose.""" + return self.__translation - if tvec.size == 3: - self.__translation = tvec + @translation.setter + def translation(self, tvec): - if rvec.size == 3: - self.__rotation = rvec + self.__translation = tvec + self.__succeded = True + self.__validity = 0 + + @property + def rotation(self) -> numpy.array: + """Access to cube rotation vector. + + .. warning:: + Setting cube rotation vector implies succeded status to be True and validity score to be 0.""" + + return self.__translation + + @rotation.setter + def rotation(self, rvec): + self.__rotation = rvec self.__succeded = True self.__validity = 0 + @property + def succeded(self) -> bool: + """Access to cube pose estimation succeded status.""" + + return self.__succeded + + @property + def validity(self) -> int: + """Access to cube pose estimation validity score.""" + + return self.__validity + def draw(self, frame, K, D, draw_faces=True): - """Draw cube axis and faces. - **Returns:** frame""" + """Draw cube axis and faces.""" l = self.edge_size / 2 ll = self.edge_size @@ -362,9 +394,9 @@ class ArUcoCube(): 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()), (n,n,f), 5) # X (red) - frame = cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 5) # Y (green) - frame = cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 5) # Z (blue) + cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 5) # X (red) + cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 5) # Y (green) + cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 5) # Z (blue) if draw_faces: @@ -373,30 +405,30 @@ class ArUcoCube(): leftPoints, _ = cv.projectPoints(leftPoints, self.__rotation, self.__translation, K, D) leftPoints = leftPoints.astype(int) - frame = cv.line(frame, tuple(leftPoints[0].ravel()), tuple(leftPoints[1].ravel()), (n,n,f), 2) - frame = cv.line(frame, tuple(leftPoints[1].ravel()), tuple(leftPoints[2].ravel()), (n,n,f), 2) - frame = cv.line(frame, tuple(leftPoints[2].ravel()), tuple(leftPoints[3].ravel()), (n,n,f), 2) - frame = cv.line(frame, tuple(leftPoints[3].ravel()), tuple(leftPoints[0].ravel()), (n,n,f), 2) + cv.line(frame, tuple(leftPoints[0].ravel()), tuple(leftPoints[1].ravel()), (n,n,f), 2) + cv.line(frame, tuple(leftPoints[1].ravel()), tuple(leftPoints[2].ravel()), (n,n,f), 2) + cv.line(frame, tuple(leftPoints[2].ravel()), tuple(leftPoints[3].ravel()), (n,n,f), 2) + cv.line(frame, tuple(leftPoints[3].ravel()), tuple(leftPoints[0].ravel()), (n,n,f), 2) # 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, D) topPoints = topPoints.astype(int) - frame = cv.line(frame, tuple(topPoints[0].ravel()), tuple(topPoints[1].ravel()), (n,f,n), 2) - frame = cv.line(frame, tuple(topPoints[1].ravel()), tuple(topPoints[2].ravel()), (n,f,n), 2) - frame = cv.line(frame, tuple(topPoints[2].ravel()), tuple(topPoints[3].ravel()), (n,f,n), 2) - frame = cv.line(frame, tuple(topPoints[3].ravel()), tuple(topPoints[0].ravel()), (n,f,n), 2) + cv.line(frame, tuple(topPoints[0].ravel()), tuple(topPoints[1].ravel()), (n,f,n), 2) + cv.line(frame, tuple(topPoints[1].ravel()), tuple(topPoints[2].ravel()), (n,f,n), 2) + cv.line(frame, tuple(topPoints[2].ravel()), tuple(topPoints[3].ravel()), (n,f,n), 2) + cv.line(frame, tuple(topPoints[3].ravel()), tuple(topPoints[0].ravel()), (n,f,n), 2) # 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, D) frontPoints = frontPoints.astype(int) - frame = cv.line(frame, tuple(frontPoints[0].ravel()), tuple(frontPoints[1].ravel()), (f,n,n), 2) - frame = cv.line(frame, tuple(frontPoints[1].ravel()), tuple(frontPoints[2].ravel()), (f,n,n), 2) - frame = cv.line(frame, tuple(frontPoints[2].ravel()), tuple(frontPoints[3].ravel()), (f,n,n), 2) - frame = cv.line(frame, tuple(frontPoints[3].ravel()), tuple(frontPoints[0].ravel()), (f,n,n), 2) + cv.line(frame, tuple(frontPoints[0].ravel()), tuple(frontPoints[1].ravel()), (f,n,n), 2) + cv.line(frame, tuple(frontPoints[1].ravel()), tuple(frontPoints[2].ravel()), (f,n,n), 2) + cv.line(frame, tuple(frontPoints[2].ravel()), tuple(frontPoints[3].ravel()), (f,n,n), 2) + cv.line(frame, tuple(frontPoints[3].ravel()), tuple(frontPoints[0].ravel()), (f,n,n), 2) except Exception as e: @@ -406,5 +438,3 @@ class ArUcoCube(): print(self.__succeded) print(self.__validity) print(axisPoints) - - return frame |