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