aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py195
1 files changed, 60 insertions, 135 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 6fe0d0f..2ca1542 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -13,6 +13,12 @@ import numpy
import cv2 as cv
import cv2.aruco as aruco
+T0 = numpy.array([0., 0., 0.])
+"""Define no translation vector."""
+
+R0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
+"""Define no rotation matrix."""
+
ArUcoSceneType = TypeVar('ArUcoScene', bound="ArUcoScene")
# Type definition for type annotation convenience
@@ -84,29 +90,12 @@ class ArUcoScene():
for name, place in self.__places.items():
self.__identifier_cache[place.marker.identifier] = name
- # Process each place pose to speed up further calculations
- self.__translation_cache = {}
- for name, place in self.__places.items():
- self.__translation_cache[name] = place.translation
-
- # Process each place rotation matrix to speed up further calculations
- self.__rotation_cache = {}
- for name, place in self.__places.items():
-
- # Create intrinsic rotation matrix
- R = self.__euler_vector_to_rotation_matrix(*place.rotation)
-
- assert(self.__is_rotation_matrix(R))
-
- # Store rotation matrix
- self.__rotation_cache[name] = R
-
# Process axis-angle between place combination to speed up further calculations
self.__angle_cache = {}
for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.__places.items(), 2):
- A = self.__rotation_cache[A_name]
- B = self.__rotation_cache[B_name]
+ A = self.__places[A_name].rotation
+ B = self.__places[B_name].rotation
if numpy.array_equal(A, B):
@@ -117,7 +106,7 @@ class ArUcoScene():
# Rotation matrix from A place to B place
AB = B.dot(A.T)
- assert(self.__is_rotation_matrix(AB))
+ #assert(self.__is_rotation_matrix(AB))
# Calculate axis-angle representation of AB rotation matrix
angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
@@ -136,8 +125,8 @@ class ArUcoScene():
self.__distance_cache = {}
for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.__places.items(), 2):
- A = self.__translation_cache[A_name]
- B = self.__translation_cache[B_name]
+ A = self.__places[A_name].translation
+ B = self.__places[B_name].translation
# Calculate axis-angle representation of AB rotation matrix
distance = numpy.linalg.norm(B - A)
@@ -169,14 +158,6 @@ class ArUcoScene():
for i, name in self.__identifier_cache.items():
output += f'\n\t\t- {i}: {name}'
- output += '\n\n\tTranslation cache:'
- for name, item in self.__translation_cache.items():
- output += f'\n\t\t- {name}: {item}'
-
- output += '\n\n\tRotation cache:'
- for name, item in self.__rotation_cache.items():
- output += f'\n\t\t- {name}:\n{item}'
-
output += '\n\n\tAngle cache:'
for A_name, A_angle_cache in self.__angle_cache.items():
for B_name, angle in A_angle_cache.items():
@@ -229,7 +210,7 @@ class ArUcoScene():
name = None
vertices = []
markers = {}
- rotations = {}
+ normals = {}
faces = {}
# Open the file and read through it line by line
@@ -268,19 +249,10 @@ class ArUcoScene():
vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]))
- # Extract normal to calculate rotation vectore
+ # Extract normal to calculate rotation matrix
elif key == 'normal':
- R = self.__normal_vector_to_rotation_matrix(float(match.group(1)), float(match.group(2)), float(match.group(3)))
- rvecs, _ = cv.Rodrigues(R)
- rvecs = rvecs.reshape(1, 3)[0]
- rvecs = numpy.array([numpy.rad2deg(rvecs[0]), numpy.rad2deg(rvecs[1]), numpy.rad2deg(rvecs[2])])
-
- print(name)
- print(R)
- print(rvecs)
-
- rotations[name] = rvecs
+ normals[name] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])
# Extract vertice ids
elif key == 'face':
@@ -295,87 +267,63 @@ class ArUcoScene():
# Retreive marker vertices thanks to face vertice ids
for name, face in faces.items():
- translation = numpy.array([ vertices[i-1] for i in face ]).mean(axis=0)
- rotation = rotations[name]
- marker = markers[name]
-
- self.__places[name] = Place(translation, rotation, marker)
-
- except IOError:
- raise IOError(f'File not found: {obj_filepath}')
-
- def __normal_vector_to_rotation_matrix(self, x, y, z):
- # Applying formula found at https://math.stackexchange.com/questions/1956699/getting-a-transformation-matrix-from-a-normal-vector
-
- xy_dist = math.sqrt(x**2 + y**2)
+ # Gather place corners from counter clockwise ordered face vertices
+ corners = numpy.array([ vertices[i-1] for i in face ])
- if xy_dist > 0:
+ # Edit place translation as corners center
+ T = corners.mean(axis=0)
- return numpy.array([[y/xy_dist, -x/xy_dist, 0], [x*z/xy_dist, y*z/xy_dist, -xy_dist], [x, y, z]])
+ # Edit place rotation from corners positions
+ marker_x_axis = corners[1:3].mean(axis=0) - T
+ marker_x_axis = marker_x_axis / numpy.linalg.norm(marker_x_axis)
- else:
+ marker_y_axis = corners[2:4].mean(axis=0) - T
+ marker_y_axis = marker_y_axis / numpy.linalg.norm(marker_y_axis)
- return numpy.array([[1, 0, 0], [0, 1, 0], [x, y, z]])
+ marker_z_axis = normals[name]
- def __euler_vector_to_rotation_matrix(self, x, y, z):
+ # RM = P
+ M = numpy.array([marker_x_axis, marker_y_axis, marker_z_axis])
+ P = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
+ R = P.dot(numpy.linalg.inv(M))
- # 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]])
+ marker = markers[name]
- # 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]])
+ self.__places[name] = Place(T, R, marker)
- # 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]])
+ except IOError:
+ raise IOError(f'File not found: {obj_filepath}')
- # 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."""
I = numpy.identity(3, dtype = R.dtype)
return numpy.linalg.norm(I - numpy.dot(R.T, R)) < 1e-6
- def __normalise_marker_pose(self, name, place, F):
-
- # Transform place rotation into scene rotation vector
- R = self.__rotation_cache[name]
- rvec, _ = cv.Rodrigues(F.dot(R))
-
- #print(f'{name} rotation vector: {rvec[0][0]:3f} {rvec[1][0]:3f} {rvec[2][0]:3f}')
-
- # Transform place translation into scene translation vector
- OF = place.translation
- T = self.__translation_cache[name]
- FC = R.dot(F.dot(-T))
+ def __normalise_marker_pose(self, place, R, T):
- tvec = OF + FC
+ # Transform marker rotation into scene rotation matrix
+ Rs = place.rotation.dot(R)
- #print(f'{name} translation vector: {tvec[0]:3f} {tvec[1]:3f} {tvec[2]:3f}')
+ # Transform marker translation into scene translation vector
+ Ts = T + place.rotation.dot(R.dot(-place.translation))
- return rvec, tvec
+ return Rs, Ts
def estimate_pose(self, tracked_markers) -> Tuple[numpy.array, numpy.array, bool, int, dict]:
"""Estimate scene pose from tracked markers (cf ArUcoTracker.track()) and validate its consistency according expected scene places.
* **Returns:**
- scene translation vector
- - scene rotation vector
+ - scene rotation matrix
- scene pose estimation success status
- all tracked markers considered as consistent and used to estimate the pose
- dict of identified distance or angle unconsistencies and out-of-bounds values
"""
# Init pose data
- self._translation = numpy.zeros(3)
- self._rotation = numpy.zeros(3)
+ self._translation = T0
+ self._rotation = R0
self._succeded = False
self._consistent_markers = {}
self._unconsistencies = {}
@@ -396,41 +344,27 @@ class ArUcoScene():
except KeyError:
continue
- #print('-------------- ArUcoScene pose estimation --------------')
-
# Pose consistency checking is'nt possible when only one marker is tracked
if len(scene_markers) == 1:
- # Get scene pose from to the unique marker
+ # Get scene pose from the unique marker
name, marker = scene_markers.popitem()
- F, _ = cv.Rodrigues(marker.rotation)
- self._rotation, self._translation = self.__normalise_marker_pose(name, marker, F)
+ self._rotation, self._translation = self.__normalise_marker_pose(self.__places[name], marker.rotation, marker.translation)
self._succeded = True
self._consistent_markers[name] = marker
- #print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
- #print(f'ArUcoScene rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
- #print(f'ArUcoScene translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
-
# Pose consistency checking processes markers two by two
else:
consistent_markers = []
- consistent_rvecs = []
+ consistent_rmats = []
consistent_tvecs = []
for (A_name, A_marker), (B_name, B_marker) in itertools.combinations(scene_markers.items(), 2):
- #print(f'** {A_name} > {B_name}')
-
- # Get marker rotation estimation
- # Use rotation matrix instead of rotation vector
- A, _ = cv.Rodrigues(A_marker.rotation)
- B, _ = cv.Rodrigues(B_marker.rotation)
-
# Rotation matrix from A marker to B marker
- AB = B.dot(A.T)
+ AB = B_marker.rotation.dot(A_marker.rotation.T)
assert(self.__is_rotation_matrix(AB))
@@ -453,10 +387,10 @@ class ArUcoScene():
# Remember this marker is already validated
consistent_markers.append(A_name)
- rvec, tvec = self.__normalise_marker_pose(A_name, A_marker, A)
+ rmat, tvec = self.__normalise_marker_pose(self.__places[A_name], A_marker.rotation, A_marker.translation)
# Store normalised marker pose
- consistent_rvecs.append(rvec)
+ consistent_rmats.append(rmat)
consistent_tvecs.append(tvec)
if B_name not in consistent_markers:
@@ -464,10 +398,10 @@ class ArUcoScene():
# Remember this marker is already validated
consistent_markers.append(B_name)
- rvec, tvec = self.__normalise_marker_pose(B_name, B_marker, B)
+ rmat, tvec = self.__normalise_marker_pose(self.__places[B_name], B_marker.rotation, B_marker.translation)
# Store normalised marker pose
- consistent_rvecs.append(rvec)
+ consistent_rmats.append(rmat)
consistent_tvecs.append(tvec)
else:
@@ -482,21 +416,17 @@ class ArUcoScene():
# Consider ArUcoScene rotation as the mean of all consistent 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(consistent_rvecs), axis=0)
+ self._rotation = numpy.mean(numpy.array(consistent_rmats), axis=0)
# Consider ArUcoScene translation as the mean of all consistent translations
self._translation = numpy.mean(numpy.array(consistent_tvecs), axis=0)
- #print(':::::::::::::::::::::::::::::::::::::::::::::::::::')
- #print(f'ArUcoScene rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
- #print(f'ArUcoScene translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
-
self._succeded = True
self._consistent_markers = consistent_markers
else:
- unconsistent_rvecs = []
+ unconsistent_rmats = []
unconsistent_tvecs = []
# Gather unconsistent pose estimations
@@ -504,23 +434,18 @@ class ArUcoScene():
if name not in consistent_markers:
- R, _ = cv.Rodrigues(marker.rotation)
- rvec, tvec = self.__normalise_marker_pose(name, marker, R)
+ rmat, tvec = self.__normalise_marker_pose(self.__places[name], marker.rotation, marker.translation)
- unconsistent_rvecs = [rvec]
+ unconsistent_rmats = [rmat]
unconsistent_tvecs = [tvec]
# Consider ArUcoScene rotation as the mean of all unconsistent 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(unconsistent_rvecs), axis=0)
+ self._rotation = numpy.mean(numpy.array(unconsistent_rmats), axis=0)
# Consider ArUcoScene translation as the mean of all unconsistent translations
self._translation = numpy.mean(numpy.array(unconsistent_tvecs), axis=0)
- #print(':::::::::::::::::::::::::::::::::::::::::::::::::::')
- #print(f'ArUcoScene rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
- #print(f'ArUcoScene translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
-
self._succeded = False
self._consistent_markers = {}
@@ -546,17 +471,17 @@ class ArUcoScene():
@property
def rotation(self) -> numpy.array:
- """Access to scene rotation vector.
+ """Access to scene rotation matrix.
.. warning::
- Setting scene rotation vector implies succeded status to be True and consistency score to be 0."""
+ Setting scene rotation matrix implies succeded status to be True and consistency score to be 0."""
return self._translation
@rotation.setter
- def rotation(self, rvec):
+ def rotation(self, rmat):
- self._rotation = rvec
+ self._rotation = rmat
self._succeded = True
self._consistent_markers = 0
@@ -601,8 +526,8 @@ class ArUcoScene():
if name != "top":
continue
- T = self.__translation_cache[name]
- R = self.__rotation_cache[name]
+ T = self.__places[name].translation
+ R = self.__places[name].rotation
placePoints = (T + numpy.float32([R.dot([-l, -l, 0]), R.dot([l, -l, 0]), R.dot([l, l, 0]), R.dot([-l, l, 0])])).reshape(-1, 3)
placePoints, _ = cv.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))