Adding ArUcoCube features and rewritting ArUcoTracker using ArUcoMarker class.

5 files changed, 447 insertions, 181 deletions

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