From 853d1b174b32c9fb67c6f7bd3b9c6f0d62c184f5 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 19 Oct 2022 18:11:14 +0200 Subject: Adding ArUcoCube features and rewritting ArUcoTracker using ArUcoMarker class. --- src/argaze/ArUcoMarkers/ArUcoCube.py | 337 ++++++++++++++++++++++ src/argaze/ArUcoMarkers/ArUcoMarker.py | 48 +++ src/argaze/ArUcoMarkers/ArUcoMarkersDictionary.py | 12 +- src/argaze/ArUcoMarkers/ArUcoTracker.py | 229 ++++----------- src/argaze/ArUcoMarkers/__init__.py | 2 +- 5 files changed, 447 insertions(+), 181 deletions(-) create mode 100644 src/argaze/ArUcoMarkers/ArUcoCube.py create mode 100644 src/argaze/ArUcoMarkers/ArUcoMarker.py diff --git a/src/argaze/ArUcoMarkers/ArUcoCube.py b/src/argaze/ArUcoMarkers/ArUcoCube.py new file mode 100644 index 0000000..7012e0a --- /dev/null +++ b/src/argaze/ArUcoMarkers/ArUcoCube.py @@ -0,0 +1,337 @@ +#!/usr/bin/env python + +from dataclasses import dataclass, field +import json +import math +import itertools + +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker + +import numpy +import cv2 as cv +import cv2.aruco as aruco + +@dataclass +class ArUcoCubeFace(): + """Define cube face pose and marker.""" + + translation: numpy.array + """Position in cube referential.""" + + rotation: numpy.array + """Rotation in cube referential.""" + + marker: dict + """ArUco marker linked to the face """ + +@dataclass +class ArUcoCube(): + """Define a cube with ArUco markers on each face and estimate its pose.""" + + dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary + """ArUco dictionary of cube markers.""" + + marker_size: int = field(init=False) + """Size of markers in centimeter.""" + + edge_size: int = field(init=False) + """Size of the cube edges in centimeter.""" + + faces: dict = field(init=False, default_factory=dict) + """All named faces of the cube and their ArUco markers.""" + + translation: numpy.ndarray = field(init=False) + """Position of the cube.""" + + rotation: numpy.ndarray = field(init=False) + """Rotation of the cube.""" + + angle_tolerance: float = field(init=False) + """Angle error tolerance allowed to validate face pose in degree.""" + + def __init__(self, configuration_filepath): + """Define cube from a .json file.""" + + with open(configuration_filepath) as configuration_file: + + # Deserialize .json + # TODO find a better way + configuration = json.load(configuration_file) + + # Load dictionary + self.dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(configuration['dictionary']) + + # Load marker size + self.marker_size = configuration['marker_size'] + + # Load edge size + self.edge_size = configuration['edge_size'] + + # Load faces + self.faces = {} + for name, face in configuration['faces'].items(): + marker = ArUcoMarker.ArUcoMarker(self.dictionary, face['marker'], self.marker_size) + self.faces[name] = ArUcoCubeFace(numpy.array(face['translation']).astype(numpy.float32), numpy.array(face['rotation']).astype(numpy.float32), marker) + + # Load angle tolerance + self.angle_tolerance = configuration['angle_tolerance'] + + # Init pose data + self.translation = numpy.zeros(3) + self.rotation = numpy.zeros(3) + + # Process markers ids to speed up further calculations + self.__identifier_cache = {} + for name, face in self.faces.items(): + self.__identifier_cache[face.marker.identifier] = name + + # Process each face pose to speed up further calculations + self.__translation_cache = {} + for name, face in self.faces.items(): + self.__translation_cache[name] = face.translation * self.edge_size / 2 + + # Process each face rotation matrix to speed up further calculations + self.__rotation_cache = {} + for name, face in self.faces.items(): + + # Create rotation matrix around x axis + c = numpy.cos(numpy.deg2rad(face.rotation[0])) + s = numpy.sin(numpy.deg2rad(face.rotation[0])) + Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) + + # Create rotation matrix around y axis + c = numpy.cos(numpy.deg2rad(face.rotation[1])) + s = numpy.sin(numpy.deg2rad(face.rotation[1])) + Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) + + # Create rotation matrix around z axis + c = numpy.cos(numpy.deg2rad(face.rotation[2])) + s = numpy.sin(numpy.deg2rad(face.rotation[2])) + Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + + # Create intrinsic rotation matrix + R = Rx.dot(Ry.dot(Rz)) + + assert(self.__is_rotation_matrix(R)) + + # Store rotation matrix + self.__rotation_cache[name] = R + + # Process each axis-angle face combination to speed up further calculations + self.__angle_cache = {} + for (A_name, A_face), (B_name, B_face) in itertools.combinations(self.faces.items(), 2): + + A = self.__rotation_cache[A_name] + B = self.__rotation_cache[B_name] + + # Rotation matrix from A face to B face + AB = B.dot(A.T) + + assert(self.__is_rotation_matrix(AB)) + + # Calculate axis-angle representation of AB rotation matrix + angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) + + try: + self.__angle_cache[A_name][B_name] = angle + except: + self.__angle_cache[A_name] = {B_name: angle} + + try: + self.__angle_cache[B_name][A_name] = angle + except: + self.__angle_cache[B_name] = {A_name: angle} + + def print_cache(self): + """Print pre-processed data.""" + + print('\nIdentifier cache:') + for i, name in self.__identifier_cache.items(): + print(f'- {i}: {name}') + + print('\nTranslation cache:') + for name, item in self.__translation_cache.items(): + print(f'- {name}: {item}') + + print('\nRotation cache:') + for name, item in self.__rotation_cache.items(): + print(f'- {name}:\n{item}') + + print('\nAngle cache:') + 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}') + + 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_face_pose(self, name, face, F): + + # Transform face rotation into cube 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 face translation into cube translation vector + OF = face.translation + T = self.__translation_cache[name] + FC = F.dot(R.dot(T)) + + tvec = OF + FC + + #print(f'{name} translation vector: {tvec[0]:3f} {tvec[1]:3f} {tvec[2]:3f}') + + return rvec, tvec + + def estimate_pose(self, tracked_markers): + + # Look for faces related to tracked markers + tracked_faces = {} + for (marker_id, marker) in tracked_markers.items(): + + try: + name = self.__identifier_cache[marker_id] + tracked_faces[name] = marker + + except KeyError: + continue + + #print('-------------- ArUcoCube pose estimation --------------') + + # Pose validity checking is'nt possible when only one face of the cube is tracked + if len(tracked_faces.keys()) == 1: + + # Get arcube pose from to the unique face pose + name, face = tracked_faces.popitem() + F, _ = cv.Rodrigues(face.rotation) + + self.rotation, self.translation = self.__normalise_face_pose(name,face, F) + + #print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!') + #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}') + + # Pose validity checking processes faces two by two + else: + + valid_faces = [] + valid_rvecs = [] + valid_tvecs = [] + + for (A_name, A_face), (B_name, B_face) in itertools.combinations(tracked_faces.items(), 2): + + #print(f'** {A_name} > {B_name}') + + # Get face rotation estimation + # Use rotation matrix instead of rotation vector + A, _ = cv.Rodrigues(A_face.rotation) + B, _ = cv.Rodrigues(B_face.rotation) + + # Rotation matrix from A face to B face + AB = B.dot(A.T) + + assert(self.__is_rotation_matrix(AB)) + + # Calculate axis-angles representation of AB rotation matrix + angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) + + #print('rotation angle:') + #print(angle) + + expected_angle = self.__angle_cache[A_name][B_name] + + #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): + + if A_name not in valid_faces: + + # Remember this face is already validated + valid_faces.append(A_name) + + rvec, tvec = self.__normalise_face_pose(A_name, A_face, A) + + # Store normalised face pose + valid_rvecs.append(rvec) + valid_tvecs.append(tvec) + + if B_name not in valid_faces: + + # Remember this face is already validated + valid_faces.append(B_name) + + rvec, tvec = self.__normalise_face_pose(B_name, B_face, B) + + # Store normalised face pose + valid_rvecs.append(rvec) + valid_tvecs.append(tvec) + + if len(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. + self.rotation = numpy.mean(numpy.array(valid_rvecs), axis=0) + + # Consider arcube translation as the mean of all valid translations + self.translation = numpy.mean(numpy.array(valid_tvecs), axis=0) + + #print(':::::::::::::::::::::::::::::::::::::::::::::::::::') + #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}') + + #print('----------------------------------------------------') + + def draw(self, frame, K): + + 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 = axisPoints.astype(int) + + frame = cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0,0,255), 5) # X (red) + frame = cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0,255,0), 5) # Y (green) + frame = cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255,0,0), 5) # Z (blue) + + # Draw left face + leftPoints = numpy.float32([[-l, l, l], [-l, -l, l], [-l, -l, -l], [-l, l, -l]]).reshape(-1, 3) + leftPoints, _ = cv.projectPoints(leftPoints, self.rotation, self.translation, K, (0, 0, 0, 0)) + leftPoints = leftPoints.astype(int) + + frame = cv.line(frame, tuple(leftPoints[0].ravel()), tuple(leftPoints[1].ravel()), (0,0,255), 2) + frame = cv.line(frame, tuple(leftPoints[1].ravel()), tuple(leftPoints[2].ravel()), (0,0,255), 2) + frame = cv.line(frame, tuple(leftPoints[2].ravel()), tuple(leftPoints[3].ravel()), (0,0,255), 2) + frame = cv.line(frame, tuple(leftPoints[3].ravel()), tuple(leftPoints[0].ravel()), (0,0,255), 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, (0, 0, 0, 0)) + topPoints = topPoints.astype(int) + + frame = cv.line(frame, tuple(topPoints[0].ravel()), tuple(topPoints[1].ravel()), (0,255,0), 2) + frame = cv.line(frame, tuple(topPoints[1].ravel()), tuple(topPoints[2].ravel()), (0,255,0), 2) + frame = cv.line(frame, tuple(topPoints[2].ravel()), tuple(topPoints[3].ravel()), (0,255,0), 2) + frame = cv.line(frame, tuple(topPoints[3].ravel()), tuple(topPoints[0].ravel()), (0,255,0), 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, (0, 0, 0, 0)) + frontPoints = frontPoints.astype(int) + + frame = cv.line(frame, tuple(frontPoints[0].ravel()), tuple(frontPoints[1].ravel()), (255,0,0), 2) + frame = cv.line(frame, tuple(frontPoints[1].ravel()), tuple(frontPoints[2].ravel()), (255,0,0), 2) + frame = cv.line(frame, tuple(frontPoints[2].ravel()), tuple(frontPoints[3].ravel()), (255,0,0), 2) + frame = cv.line(frame, tuple(frontPoints[3].ravel()), tuple(frontPoints[0].ravel()), (255,0,0), 2) + + return frame + + + diff --git a/src/argaze/ArUcoMarkers/ArUcoMarker.py b/src/argaze/ArUcoMarkers/ArUcoMarker.py new file mode 100644 index 0000000..4f716bb --- /dev/null +++ b/src/argaze/ArUcoMarkers/ArUcoMarker.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python + +from dataclasses import dataclass, field + +from argaze.ArUcoMarkers import ArUcoMarkersDictionary + +import numpy +import cv2 as cv +import cv2.aruco as aruco + +@dataclass +class ArUcoMarker(): + """Define ArUco marker class.""" + + dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary + """ """ + + identifier: int + """ """ + + size: float + """Size of marker in centimeters.""" + + corners: numpy.array = field(init=False, repr=False) + """Estimated 2D corner positions in camera image referential.""" + + translation: numpy.array = field(init=False, repr=False) + """Estimated 3D center position in camera referential.""" + + rotation: numpy.array = field(init=False, repr=False) + """Estimated 3D marker rotation in camera referential.""" + + points: numpy.array = field(init=False, repr=False) + """Estimated 3D corners positions in camera referential.""" + + def center(self, i): + """Get 2D center position in camera image referential.""" + return self.corners[0].mean(axis=0) + + def draw(self, frame, K, D): + """Draw marker in frame.""" + + # Draw marker axis if pose has been estimated + if self.translation.size == 3 and self.rotation.size == 3: + + cv.drawFrameAxes(frame, K, D, self.rotation, self.translation, self.size) + + aruco.drawDetectedMarkers(frame, [self.corners], numpy.array([self.identifier])) \ No newline at end of file diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersDictionary.py b/src/argaze/ArUcoMarkers/ArUcoMarkersDictionary.py index 93c59b8..604220c 100644 --- a/src/argaze/ArUcoMarkers/ArUcoMarkersDictionary.py +++ b/src/argaze/ArUcoMarkers/ArUcoMarkersDictionary.py @@ -32,12 +32,14 @@ all_aruco_markers_dictionaries = { class ArUcoMarkersDictionary(): """Handle an ArUco markers dictionary.""" - def __init__(self, aruco_dictionary_name): + def __init__(self, name): - if all_aruco_markers_dictionaries.get(aruco_dictionary_name, None) is None: - raise NameError(f'Bad ArUco markers dictionary name: {aruco_dictionary_name}') + if all_aruco_markers_dictionaries.get(name, None) is None: + raise NameError(f'Bad ArUco markers dictionary name: {name}') - dict_name_split = aruco_dictionary_name.split('_') + self.name = name + + dict_name_split = name.split('_') self.__format = dict_name_split[1] @@ -77,7 +79,7 @@ class ArUcoMarkersDictionary(): self.__number = int(dict_name_split[2]) - self.__aruco_dict = aruco.Dictionary_get(all_aruco_markers_dictionaries[aruco_dictionary_name]) + self.__aruco_dict = aruco.Dictionary_get(all_aruco_markers_dictionaries[self.name]) def create_marker(self, i, dpi=300): """Create a marker image.""" diff --git a/src/argaze/ArUcoMarkers/ArUcoTracker.py b/src/argaze/ArUcoMarkers/ArUcoTracker.py index b887812..f5b1715 100644 --- a/src/argaze/ArUcoMarkers/ArUcoTracker.py +++ b/src/argaze/ArUcoMarkers/ArUcoTracker.py @@ -3,7 +3,7 @@ import json from collections import Counter -from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoCamera +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoCamera import numpy import cv2 as cv @@ -45,14 +45,14 @@ ArUcoTrackerParameters = [ class ArUcoTracker(): """Track ArUco markers into a frame.""" - def __init__(self, aruco_dictionary_name: str, marker_length: float, camera: ArUcoCamera.ArUcoCamera): + def __init__(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary, marker_size: float, camera: ArUcoCamera.ArUcoCamera): """Define which markers library to track and their size""" # load ArUco markers dictionary - self.__aruco_dict = ArUcoMarkersDictionary.ArUcoMarkersDictionary(aruco_dictionary_name) + self.__dictionary = dictionary # define marker length in centimeter - self.__marker_length = marker_length + self.__marker_size = marker_size # define camera self.__camera = camera @@ -62,19 +62,8 @@ class ArUcoTracker(): self.__detector_parameters.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR # to get a better pose estimation self.__detector_parameters_loaded = {} - # define tracked markers data - self.__markers_corners = [] - self.__markers_ids = [] - self.__rvecs = [] - self.__tvecs = [] - self.__points = [] - - # define rejected markers data - self.__rejected_markers_corners = [] - self.__rejected_markers_ids = [] - self.__rejected_rvecs = [] - self.__rejected_tvecs = [] - self.__rejected_points = [] + # init tracked markers data + self.__tracked_markers = {} # define tracked board data self.__board = None @@ -84,7 +73,7 @@ class ArUcoTracker(): # define track metrics data self.__track_count = 0 - self.__tracked_markers = [] + self.__tracked_ids = [] self.__rejected_markers = [] def load_configuration_file(self, configuration_filepath): @@ -115,98 +104,56 @@ class ArUcoTracker(): def track(self, frame, estimate_pose = True, check_rotation = False): """Track ArUco markers in frame.""" + self.__tracked_markers = {} + markers_corners, markers_ids, markers_rvecs, markers_tvecs, markers_points = [], [], [], [], [] + # DON'T MIRROR FRAME : it makes the markers detection to fail - # detect markers from gray picture - gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY) - self.__markers_corners, self.__markers_ids, rejectedPoints = aruco.detectMarkers(gray, self.__aruco_dict.get_markers(), parameters = self.__detector_parameters) - self.__rejected_markers_corners, __rejected_markers_ids = [], [] - - if len(self.__markers_corners) > 0 and estimate_pose: - - # markers pose estimation - self.__rvecs, self.__tvecs, self.__points = aruco.estimatePoseSingleMarkers(self.__markers_corners, self.__marker_length, self.__camera.get_K(), self.__camera.get_D()) - - # optional: check marker rotation as described in [this issue](https://github.com/opencv/opencv/issues/8813) - if check_rotation: - - valid_rotation_markers = [] - bad_rotation_markers = [] - for i, rvec in enumerate(self.__rvecs): - - tvec = self.__tvecs[i][0] - R, _ = cv.Rodrigues(rvec) - - zAxisPoint = (tvec.dot(R) + numpy.array([0., 0., 1.])).dot(R.T) - zAxis = zAxisPoint - tvec - - # TODO: How to describe the expected Z axis orientation ? - # In some situations, you can't provide such information. - - # !!! Here a description specific to SimOne cockpit !!! - zAxisExpectedDict = { - 1: numpy.array([0.5, 0.5, -1]), - 2: numpy.array([0.5, 0.5, -1]), - 3: numpy.array([1, -1, -1]), - 4: numpy.array([1, -1, -1]), - 5: numpy.array([1, -1, -1]), - 6: numpy.array([1, 1, -1]), - 7: numpy.array([1, 1, -1]), - 8: numpy.array([1, -1, -1]) - } - - zAxisExpected = zAxisExpectedDict[self.__markers_ids[i][0]] - - cosine_angle = numpy.dot(zAxis/numpy.linalg.norm(zAxis), zAxisExpected) - degree_angle = numpy.rad2deg(numpy.arccos(cosine_angle)) - ''' - print(self.__markers_ids[i][0]) - print('marker position: ', tvec) - print('zAxisPoint: ', zAxisPoint) - print('zAxis: ', zAxis) - print('zAxisExpected: ', zAxisExpected) - print('cosine_angle: ', cosine_angle) - print('degree_angle: ', degree_angle) - ''' - # Is the marker oriented as expected ? - if cosine_angle < 0 or cosine_angle > 1: - - #print('valid') - valid_rotation_markers.append(i) - - else: - - #print('bad') - bad_rotation_markers.append(i) - - # update track metrics - self.__rejected_markers.append(self.__markers_ids[i][0]) - - # keep markers with bad rotation - self.__rejected_markers_corners = tuple([self.__markers_corners[i] for i in bad_rotation_markers]) - self.__rejected_markers_ids = self.__markers_ids[bad_rotation_markers] - self.__rejected_rvecs = self.__rvecs[bad_rotation_markers] - self.__rejected_tvecs = self.__tvecs[bad_rotation_markers] - self.__rejected_points = self.__points[bad_rotation_markers] - - # keep markers with valid rotation - self.__markers_corners = tuple([self.__markers_corners[i] for i in valid_rotation_markers]) - self.__markers_ids = self.__markers_ids[valid_rotation_markers] - self.__rvecs = self.__rvecs[valid_rotation_markers] - self.__tvecs = self.__tvecs[valid_rotation_markers] - self.__points = self.__points[valid_rotation_markers] + # Track markers into gray picture + markers_corners, markers_ids, _ = aruco.detectMarkers(cv.cvtColor(frame, cv.COLOR_BGR2GRAY), self.__dictionary.get_markers(), parameters = self.__detector_parameters) - else: + if len(markers_corners) > 0: + + # Pose estimation is optional + if estimate_pose: + + markers_rvecs, markers_tvecs, markers_points = aruco.estimatePoseSingleMarkers(markers_corners, self.__marker_size, self.__camera.get_K(), self.__camera.get_D()) + + # Gather tracked markers data and update metrics + self.__track_count += 1 + + for i, marker_id in enumerate(markers_ids.T[0]): + + marker = ArUcoMarker.ArUcoMarker(self.__dictionary, marker_id, self.__marker_size) - self.__rvecs = [] - self.__tvecs = [] - self.__points = [] + marker.corners = markers_corners[i] - # update track metrics - self.__track_count += 1 - for marker_id in self.get_markers_ids(): - self.__tracked_markers.append(marker_id) + if estimate_pose: + marker.translation = markers_tvecs[i][0] + marker.rotation = markers_rvecs[i] + marker.points = markers_points[i] + self.__tracked_markers[marker_id] = marker + + self.__tracked_ids.append(marker_id) + + def get_tracked_markers(self): + """Access to tracked markers dictionary.""" + + return self.__tracked_markers + + def get_tracked_markers_number(self): + """Return tracked markers number.""" + + return len(list(self.__tracked_markers.keys())) + + def draw_tracked_markers(self, frame): + """Draw traked markers.""" + + for marker_id, marker in self.__tracked_markers.items(): + + marker.draw(frame, self.__camera.get_K(), self.__camera.get_D()) + def track_board(self, frame, board, expected_markers_number): """Track ArUco markers board in frame setting up the number of detected markers needed to agree detection.""" @@ -214,7 +161,7 @@ class ArUcoTracker(): # detect markers from gray picture gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY) - self.__markers_corners, self.__markers_ids, rejectedPoints = aruco.detectMarkers(gray, self.__aruco_dict.get_markers(), parameters = self.__detector_parameters) + self.__markers_corners, self.__markers_ids, rejectedPoints = aruco.detectMarkers(gray, self.__dictionary.get_markers(), parameters = self.__detector_parameters) # if all board markers are detected if self.get_markers_number() == expected_markers_number: @@ -229,33 +176,6 @@ class ArUcoTracker(): self.__board_corners = [] self.__board_corners_ids = [] - def draw(self, frame): - """Draw tracked markers in frame.""" - - # draw detected markers square - if len(self.__markers_corners) > 0: - - # draw marker axis if pose has been estimated - if len(self.__rvecs) > 0: - - for (i, marker_id) in enumerate(self.__markers_ids): - - cv.drawFrameAxes(frame, self.__camera.get_K(), self.__camera.get_D(), self.__rvecs[i], self.__tvecs[i], self.__marker_length) - - aruco.drawDetectedMarkers(frame, self.__markers_corners, self.__markers_ids) - - # draw rejected markers square - if len(self.__rejected_markers_corners) > 0: - - # draw marker axis if pose has been estimated - if len(self.__rejected_rvecs) > 0: - - for (i, marker_id) in enumerate(self.__rejected_markers_ids): - - cv.drawFrameAxes(frame, self.__camera.get_K(), self.__camera.get_D(), self.__rejected_rvecs[i], self.__rejected_tvecs[i], self.__marker_length) - - aruco.drawDetectedMarkers(frame, self.__rejected_markers_corners, self.__rejected_markers_ids, borderColor=(0, 255, 255)) - def draw_board(self, frame): """Draw tracked board corners in frame.""" @@ -266,52 +186,11 @@ class ArUcoTracker(): def reset_track_metrics(self): """Enable marker tracking metrics.""" self.__track_count = 0 - self.__tracked_markers = [] + self.__tracked_ids = [] def get_track_metrics(self): """Get marker tracking metrics.""" - return self.__track_count, Counter(self.__tracked_markers), Counter(self.__rejected_markers) - - def get_markers_dictionay(self): - """Get tracked aruco markers dictionary.""" - return self.__aruco_dict - - def get_markers_number(self): - """Get tracked markers number.""" - return len(self.__markers_corners) - - def get_markers_ids(self): - """Get tracked markers identifers.""" - if self.__markers_ids is not None: - return [i[0] for i in self.__markers_ids] - else: - return [] - - def get_marker_index(self, marker_id): - """Retreive marker index of a given merker id. - Raise ValueError if not found.""" - - return list(self.__markers_ids).index(marker_id) - - def get_marker_corners(self, i): - """Get marker i corners.""" - return self.__markers_corners[i][0] - - def get_marker_center(self, i): - """Get marker i center coordinates.""" - return self.__markers_corners[i][0].mean(axis=0) - - def get_marker_rotation(self, i): - """Get marker i rotation vector.""" - return self.__rvecs[i] - - def get_marker_translation(self, i): - """Get marker i translation vector.""" - return self.__tvecs[i] - - def get_marker_points(self, i): - """Get marker i points.""" - return self.__points[i] + return self.__track_count, Counter(self.__tracked_ids), Counter(self.__rejected_markers) def get_board_corners_number(self): """Get tracked board corners number.""" diff --git a/src/argaze/ArUcoMarkers/__init__.py b/src/argaze/ArUcoMarkers/__init__.py index 28386d2..3a74eeb 100644 --- a/src/argaze/ArUcoMarkers/__init__.py +++ b/src/argaze/ArUcoMarkers/__init__.py @@ -2,4 +2,4 @@ .. include:: README.md """ __docformat__ = "restructuredtext" -__all__ = ['ArUcoMarkersDictionary', 'ArUcoBoard', 'ArUcoCamera', 'ArUcoTracker'] \ No newline at end of file +__all__ = ['ArUcoMarkersDictionary', 'ArUcoMarker', 'ArUcoBoard', 'ArUcoCamera', 'ArUcoTracker', 'ArUcoCube'] \ No newline at end of file -- cgit v1.1