diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 264 |
1 files changed, 19 insertions, 245 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py index c4a2930..04c47c5 100644 --- a/src/argaze/utils/tobii_stream_arcube_display.py +++ b/src/argaze/utils/tobii_stream_arcube_display.py @@ -12,30 +12,10 @@ from argaze.utils import MiscFeatures import cv2 as cv import numpy -import math -import itertools -def isRotationMatrix(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 draw_axis(img, rvec, tvec, K): - - points = numpy.float32([[6, 0, 0], [0, 6, 0], [0, 0, 6], [0, 0, 0]]).reshape(-1, 3) - axisPoints, _ = cv.projectPoints(points, rvec, tvec, K, (0, 0, 0, 0)) - axisPoints = axisPoints.astype(int) - - img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (255,0,0), 5) - img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0,255,0), 5) - img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (0,0,255), 5) - - return img - def main(): """ - Track ArCube into Tobii Glasses Pro 2 camera video stream. + Track ArUcoCube into Tobii Glasses Pro 2 camera video stream. """ # Manage arguments @@ -43,87 +23,17 @@ def main(): parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip') parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default=None, help='json camera calibration filepath') parser.add_argument('-p', '--aruco_tracker_configuration', metavar='TRACK_CONFIG', type=str, default=None, help='json aruco tracker configuration filepath') - parser.add_argument('-ac', '--arcube', metavar='ARCUBE', type=str, help='json arcube description filepath') + parser.add_argument('-ac', '--aruco_cube', metavar='ARUCO_CUBE', type=str, help='json arcube description filepath') parser.add_argument('-to', '--tolerance', metavar='TOLERANCE', type=float, default=1, help='arcube face pose estimation tolerance') parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction) args = parser.parse_args() - # Load ArCube json description - with open(args.arcube) as arcube_file: - arcube = json.load(arcube_file) - - # Process each face translation vector to speed up further calculations - arcube_size = arcube['size'] - for face, distances in arcube['translations'].items(): - - # Create translation vector - T = numpy.array([distances['x'], distances['y'], distances['z']]) * arcube_size / 2 - - # Store translation vector - arcube['translations'][face]['vector'] = T - - print(f'*** {face}') - print('translation vector:') - print(T) - - # Process each face rotation matrix to speed up further calculations - for face, angles in arcube['rotations'].items(): - - # Create rotation matrix around x axis - c = numpy.cos(numpy.deg2rad(angles['x'])) - s = numpy.sin(numpy.deg2rad(angles['x'])) - Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) - - # Create rotation matrix around y axis - c = numpy.cos(numpy.deg2rad(angles['y'])) - s = numpy.sin(numpy.deg2rad(angles['y'])) - Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) - - # Create rotation matrix around z axis - c = numpy.cos(numpy.deg2rad(angles['z'])) - s = numpy.sin(numpy.deg2rad(angles['z'])) - Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) - - # Create intrinsic rotation matrix - R = Rx.dot(Ry.dot(Rz)) - - assert(isRotationMatrix(R)) - - # Store rotation matrix - arcube['rotations'][face]['matrix'] = R - - print(f'*** {face}') - print('rotation matrix:') - print(R) - - # Process each axis-angle face combination to speed up further calculations - for (A_face, A_item), (B_face, B_item) in itertools.combinations(arcube['rotations'].items(), 2): + # Load ArUcoCube + arcube = ArUcoCube.ArUcoCube(args.aruco_cube) + arcube.print_cache() - print(f'** {A_face} > {B_face}') - - A = A_item['matrix'] - B = B_item['matrix'] - - # Rotation matrix from A face to B face - AB = B.dot(A.T) - - assert(isRotationMatrix(AB)) - - # Calculate axis-angle representation of AB rotation matrix - angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) - - arcube['rotations'][A_face][B_face] = angle - - print('rotation angle:') - print(angle) - - # Manage ArCube markers id to track - arcube_ids = list(arcube['markers']['ids'].values()) - arcube_dictionary = arcube['markers']['dictionary'] - print(f'Track Aruco markers {arcube_ids} from the {arcube_dictionary} dictionary') - # Create tobii controller (with auto discovery network process if no ip argument is provided) - print("Looking for a Tobii Glasses Pro 2 device ...") + print('\nLooking for a Tobii Glasses Pro 2 device ...') try: @@ -154,14 +64,14 @@ def main(): raise UserWarning('.json camera calibration filepath required. Use -c option.') # Create aruco tracker - aruco_tracker = ArUcoTracker.ArUcoTracker(arcube['markers']['dictionary'], arcube['markers']['size'], aruco_camera) + aruco_tracker = ArUcoTracker.ArUcoTracker(arcube.dictionary, arcube.marker_size, aruco_camera) # Load specific configuration file if args.aruco_tracker_configuration != None: aruco_tracker.load_configuration_file(args.aruco_tracker_configuration) - print(f'ArUcoTracker configuration for {aruco_tracker.get_markers_dictionay().get_markers_format()} markers detection:') + print(f'ArUcoTracker configuration for markers detection:') aruco_tracker.print_configuration() # Init head pose tracking @@ -221,158 +131,22 @@ def main(): # Process video and data frame try: - # Track markers with pose estimation and draw them + # Track markers with pose estimation aruco_tracker.track(video_frame.matrix) - #aruco_tracker.draw(visu_frame.matrix) - - # Pose can't be estimated from markers - if aruco_tracker.get_markers_number() == 0: - - raise UserWarning('No marker detected') - - # Look for ArCube's faces among tracked markers and store their pose - arcube_tracked_faces = {} - for (face, marker_id) in arcube['markers']['ids'].items(): - - try: - marker_index = aruco_tracker.get_marker_index(marker_id) - - arcube_tracked_faces[face] = {} - arcube_tracked_faces[face]['rotation'] = aruco_tracker.get_marker_rotation(marker_index) - arcube_tracked_faces[face]['translation'] = aruco_tracker.get_marker_translation(marker_index)[0] - - except ValueError: - continue - - print('-------------- ArCube pose estimation --------------') - - # Pose validity check is'nt possible when only one face of the cube is tracked - if len(arcube_tracked_faces.keys()) == 1: - - # Get arcube pose from to the unique face pose - face, pose = arcube_tracked_faces.popitem() - # Transform face rotation into cube rotation vector - F, _ = cv.Rodrigues(pose['rotation']) - R = arcube['rotations'][face]['matrix'] - arcube_rvec, _ = cv.Rodrigues(F.dot(R)) + # Draw markers pose estimation + aruco_tracker.draw_tracked_markers(visu_frame.matrix) - # Transform face translation into cube translation vector - OF = pose['translation'] - T = arcube['translations'][face]['vector'] - FC = F.dot(R.dot(T)) + # Warn user that no markers have been detected + if aruco_tracker.get_tracked_markers_number() == 0: - arcube_tvec = OF + FC - - print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!') - print(f'arcube rotation vector: {arcube_rvec[0][0]:3f} {arcube_rvec[1][0]:3f} {arcube_rvec[2][0]:3f}') - print(f'arcube translation vector: {arcube_tvec[0]:3f} {arcube_tvec[1]:3f} {arcube_tvec[2]:3f}') - - draw_axis(visu_frame.matrix, arcube_rvec, arcube_tvec, aruco_camera.get_K()) - - # Check faces pose validity two by two - else: - - arcube_valid_faces = [] - arcube_valid_rvecs = [] - arcube_valid_tvecs = [] - - for (A_face, A_pose), (B_face, B_pose) in itertools.combinations(arcube_tracked_faces.items(), 2): - - #print(f'** {A_face} > {B_face}') - - # Get face rotation estimation - # Use rotation matrix instead of rotation vector - A, _ = cv.Rodrigues(A_pose['rotation']) - B, _ = cv.Rodrigues(B_pose['rotation']) - - # Rotation matrix from A face to B face - AB = B.dot(A.T) - - assert(isRotationMatrix(AB)) - - # Calculate axis-angles representation of AB rotation matrix - angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) - - #print('rotation angle:') - #print(angle) - - try: - expected_angle = arcube['rotations'][A_face][B_face] - - except KeyError: - expected_angle = arcube['rotations'][B_face][A_face] - - #print('expected angle:') - #print(expected_angle) - - # Check angle according given tolerance then normalise face rotation - if math.isclose(angle, expected_angle, abs_tol=args.tolerance): - - if A_face not in arcube_valid_faces: - - # Remember this face is already validated - arcube_valid_faces.append(A_face) - - # Transform face rotation into cube rotation vector - R = arcube['rotations'][A_face]['matrix'] - rvec, _ = cv.Rodrigues(A.dot(R)) - - #print(f'{A_face} rotation vector: {rvec[0][0]:3f} {rvec[1][0]:3f} {rvec[2][0]:3f}') - - # Transform face translation into cube translation vector - OA = A_pose['translation'] - T = arcube['translations'][A_face]['vector'] - AC = A.dot(R.dot(T)) - - tvec = OA + AC - - #print(f'{A_face} translation vector: {tvec[0]:3f} {tvec[1]:3f} {tvec[2]:3f}') - - # Store normalised face pose - arcube_valid_rvecs.append(rvec) - arcube_valid_tvecs.append(tvec) - - if B_face not in arcube_valid_faces: - - # Remember this face is already validated - arcube_valid_faces.append(B_face) - - # Normalise face rotation - R = arcube['rotations'][B_face]['matrix'] - rvec, _ = cv.Rodrigues(B.dot(R)) - - #print(f'{B_face} rotation vector: {rvec[0][0]:3f} {rvec[1][0]:3f} {rvec[2][0]:3f}') - - # Normalise face translation - OB = B_pose['translation'] - T = arcube['translations'][B_face]['vector'] - BC = B.dot(R.dot(T)) - - tvec = OB + BC - - #print(f'{B_face} translation vector: {tvec[0]:3f} {tvec[1]:3f} {tvec[2]:3f}') - - # Store normalised face pose - arcube_valid_rvecs.append(rvec) - arcube_valid_tvecs.append(tvec) - - if len(arcube_valid_faces) > 1: - - # 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. - arcube_rvec = numpy.mean(numpy.array(arcube_valid_rvecs), axis=0) - - # Consider arcube translation as the mean of all valid translations - arcube_tvec = numpy.mean(numpy.array(arcube_valid_tvecs), axis=0) - - print(':::::::::::::::::::::::::::::::::::::::::::::::::::') - print(f'arcube rotation vector: {arcube_rvec[0][0]:3f} {arcube_rvec[1][0]:3f} {arcube_rvec[2][0]:3f}') - print(f'arcube translation vector: {arcube_tvec[0]:3f} {arcube_tvec[1]:3f} {arcube_tvec[2]:3f}') + raise UserWarning('No marker detected') - draw_axis(visu_frame.matrix, arcube_rvec, arcube_tvec, aruco_camera.get_K()) + # Estimate cube pose from tracked markers + arcube.estimate_pose(aruco_tracker.get_tracked_markers()) - print('----------------------------------------------------') + # Draw cube axis + arcube.draw(visu_frame.matrix, aruco_camera.get_K()) # Write warning except UserWarning as w: @@ -399,7 +173,7 @@ def main(): cv.putText(visu_frame.matrix, f'Video delay: {int(data_ts_ms - video_ts_ms)} ms', (550, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) cv.putText(visu_frame.matrix, f'Fps: {int(loop_ps)}', (950, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA) - cv.imshow(f'Stream ArCube', visu_frame.matrix) + cv.imshow(f'Stream ArUcoCube', visu_frame.matrix) # Close window using 'Esc' key if cv.waitKey(1) == 27: |