aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorThéo de la Hogue2022-10-19 19:01:12 +0200
committerThéo de la Hogue2022-10-19 19:01:12 +0200
commiteaf43baf468d5bf007e6f6b3c9b2033e9fbb332b (patch)
tree02891558a4476d6c6e97a00632975c502cd12e43
parent7d9f722bf00f07631a9b052549da107b132c5276 (diff)
downloadargaze-eaf43baf468d5bf007e6f6b3c9b2033e9fbb332b.zip
argaze-eaf43baf468d5bf007e6f6b3c9b2033e9fbb332b.tar.gz
argaze-eaf43baf468d5bf007e6f6b3c9b2033e9fbb332b.tar.bz2
argaze-eaf43baf468d5bf007e6f6b3c9b2033e9fbb332b.tar.xz
checking distance to validate cube pose.
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoCube.py39
1 files changed, 30 insertions, 9 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoCube.py b/src/argaze/ArUcoMarkers/ArUcoCube.py
index 7012e0a..552d4d1 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCube.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCube.py
@@ -40,15 +40,18 @@ class ArUcoCube():
faces: dict = field(init=False, default_factory=dict)
"""All named faces of the cube and their ArUco markers."""
- translation: numpy.ndarray = field(init=False)
+ translation: numpy.array = field(init=False)
"""Position of the cube."""
- rotation: numpy.ndarray = field(init=False)
+ 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."""
+ distance_tolerance: float = field(init=False)
+ """Distance error tolerance allowed to validate face pose in centimeter."""
+
def __init__(self, configuration_filepath):
"""Define cube from a .json file."""
@@ -76,6 +79,9 @@ class ArUcoCube():
# Load angle tolerance
self.angle_tolerance = configuration['angle_tolerance']
+ # Load distance tolerance
+ self.distance_tolerance = configuration['distance_tolerance']
+
# Init pose data
self.translation = numpy.zeros(3)
self.rotation = numpy.zeros(3)
@@ -142,6 +148,9 @@ class ArUcoCube():
except:
self.__angle_cache[B_name] = {A_name: angle}
+ # Process distance between face combination to speed up further calculations
+ self.__distance_cache = numpy.linalg.norm(numpy.array([0, 0, self.edge_size/2]) - numpy.array([0, self.edge_size/2, 0]))
+
def print_cache(self):
"""Print pre-processed data."""
@@ -161,6 +170,8 @@ class ArUcoCube():
for A_name, A_angle_cache in self.__angle_cache.items():
for B_name, angle in A_angle_cache.items():
print(f'- {A_name}/{B_name}: {angle:3f}')
+
+ print(f'\nDistance cache: {self.__distance_cache}')
def __is_rotation_matrix(self, R):
"""Checks if a matrix is a valid rotation matrix."""
@@ -215,6 +226,8 @@ class ArUcoCube():
#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
+
# Pose validity checking processes faces two by two
else:
@@ -247,8 +260,15 @@ class ArUcoCube():
#print('expected angle:')
#print(expected_angle)
- # Check angle according given tolerance then normalise face pose
- if math.isclose(angle, expected_angle, abs_tol=self.angle_tolerance):
+ # Calculate distance between A face center and B face center
+ distance = numpy.linalg.norm(A_face.translation - B_face.translation)
+ expected_distance = self.__distance_cache
+
+ # Check angle and distance according given tolerance then normalise face pose
+ valid_angle = math.isclose(angle, expected_angle, abs_tol=self.angle_tolerance)
+ valid_distance = math.isclose(distance, expected_distance, abs_tol=self.distance_tolerance)
+
+ if valid_angle and valid_distance:
if A_name not in valid_faces:
@@ -285,16 +305,20 @@ class ArUcoCube():
#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
+
+ raise ValueError('Cube pose can\'t be estimated.')
+
#print('----------------------------------------------------')
- def draw(self, frame, K):
+ def draw(self, frame, K, D):
l = self.edge_size / 2
ll = self.edge_size
# 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, (0, 0, 0, 0))
+ 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)
@@ -332,6 +356,3 @@ class ArUcoCube():
frame = cv.line(frame, tuple(frontPoints[3].ravel()), tuple(frontPoints[0].ravel()), (255,0,0), 2)
return frame
-
-
-