Major pose estimation improvement using SolvePnP algorithm.

5 files changed, 66 insertions, 363 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py
index b850dde..ed6c619 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCamera.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py
@@ -189,11 +189,8 @@ class ArUcoCamera(ArFeatures.ArCamera):
 
 			try:
 
-				# Estimate scene markers poses
-				self.aruco_detector.estimate_markers_pose(scene.aruco_markers_group.identifiers)
-
 				# Estimate scene pose from detected scene markers
-				tvec, rmat, _, _ = scene.estimate_pose(self.aruco_detector.detected_markers)
+				tvec, rmat, _ = scene.estimate_pose(self.aruco_detector.detected_markers)
 
 				# Project scene into camera frame according estimated pose
 				for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov):
diff --git a/src/argaze/ArUcoMarkers/ArUcoDetector.py b/src/argaze/ArUcoMarkers/ArUcoDetector.py
index 9e40561..f178a20 100644
--- a/src/argaze/ArUcoMarkers/ArUcoDetector.py
+++ b/src/argaze/ArUcoMarkers/ArUcoDetector.py
@@ -138,20 +138,17 @@ class ArUcoDetector():
         marker_size: Size of ArUco markers to detect in centimeter.
         optic_parameters: Optic parameters to use for ArUco detection into image.
         parameters: ArUco detector parameters.
-        smooth_marker_corners: Enable marker corners smoothing to stabilize pose estimation.
     """
 
     dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary)
     marker_size: float = field(default=0.)
     optic_parameters: ArUcoOpticCalibrator.OpticParameters = field(default_factory=ArUcoOpticCalibrator.OpticParameters)
     parameters: DetectorParameters = field(default_factory=DetectorParameters)
-    smooth_marker_corners: bool = field(default=False)
 
     def __post_init__(self):
 
         # Init detected markers data
         self.__detected_markers = {}
-        self.__last_detected_markers = {}
 
         # Init detected board data
         self.__board = None
@@ -267,9 +264,6 @@ class ArUcoDetector():
             detection time: marker detection time in ms.
         """
 
-        # Copy last detected markers
-        self.__last_detected_markers = self.__detected_markers
-
         # Reset detected markers data
         self.__detected_markers, detected_markers_corners, detected_markers_ids = {}, [], []
 
@@ -294,28 +288,7 @@ class ArUcoDetector():
             for i, marker_id in enumerate(detected_markers_ids):
 
                 marker = ArUcoMarker.ArUcoMarker(self.dictionary, marker_id, self.marker_size)
-
-                # Smooth marker corners if required
-                if self.smooth_marker_corners:
-
-                    # Try to smooth corners with last detected markers corners
-                    try:
-
-                        # Smooth corners positions if the distance between new marker and last marker is lower than half marker size
-                        half_marker_size_px = numpy.linalg.norm(detected_markers_corners[i][0][1] - detected_markers_corners[i][0][0]) / 2
-                        distance_to_last = numpy.linalg.norm(detected_markers_corners[i] - self.__last_detected_markers[marker_id].corners)
-                        smooth_factor = 0. if distance_to_last > half_marker_size_px else (half_marker_size_px - distance_to_last) / half_marker_size_px
-
-                        marker.corners = numpy.rint(self.__last_detected_markers[marker_id].corners * smooth_factor + detected_markers_corners[i] * (1 - smooth_factor))
-
-                    # Avoid smoothing if the marker was not part of last detection
-                    except KeyError:
-
-                        marker.corners = detected_markers_corners[i]
-
-                else:
-
-                    marker.corners = detected_markers_corners[i]
+                marker.corners = detected_markers_corners[i][0]
 
                 # No pose estimation: call estimate_markers_pose to get one
                 marker.translation = numpy.empty([0])
@@ -329,41 +302,6 @@ class ArUcoDetector():
 
         return detection_time
 
-    def estimate_markers_pose(self, markers_ids: list = []):
-        """Estimate pose of current detected markers or of given markers id list."""
-
-        # Is there detected markers ?
-        if len(self.__detected_markers) > 0:
-
-            # Select all markers by default
-            if len(markers_ids) == 0:
-
-                markers_ids = self.__detected_markers.keys()
-
-            # Prepare data for aruco.estimatePoseSingleMarkers function
-            selected_markers_corners = tuple()
-            selected_markers_ids = []
-
-            for marker_id, marker in self.__detected_markers.items():
-
-                if marker_id in markers_ids:
-
-                    selected_markers_corners += (marker.corners,)
-                    selected_markers_ids.append(marker_id)
-
-            # Estimate pose of selected markers
-            if len(selected_markers_corners) > 0:
-
-                markers_rvecs, markers_tvecs, markers_points = aruco.estimatePoseSingleMarkers(selected_markers_corners, self.marker_size, numpy.array(self.optic_parameters.K), numpy.array(self.optic_parameters.D)) 
-
-                for i, marker_id in enumerate(selected_markers_ids):
-
-                    marker = self.__detected_markers[marker_id]
-
-                    marker.translation = markers_tvecs[i][0]
-                    marker.rotation, _ = cv.Rodrigues(markers_rvecs[i][0])
-                    marker.points = markers_points.reshape(4, 3)
-
     @property
     def detected_markers(self) -> dict[ArUcoMarkerType]:
         """Access to detected markers dictionary."""
diff --git a/src/argaze/ArUcoMarkers/ArUcoMarker.py b/src/argaze/ArUcoMarkers/ArUcoMarker.py
index 57bd8bd..f088dae 100644
--- a/src/argaze/ArUcoMarkers/ArUcoMarker.py
+++ b/src/argaze/ArUcoMarkers/ArUcoMarker.py
@@ -68,7 +68,7 @@ class ArUcoMarker():
         # Draw marker if required
         if color is not None:
 
-            aruco.drawDetectedMarkers(image, [self.corners], numpy.array([self.identifier]), color)
+            aruco.drawDetectedMarkers(image, [numpy.array([list(self.corners)])], numpy.array([self.identifier]), color)
 
         # Draw marker axes if pose has been estimated and if required
         if self.translation.size == 3 and self.rotation.size == 9 and draw_axes is not None:
diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
index 4a43965..df390b4 100644
--- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
+++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
@@ -17,8 +17,7 @@ import re
 from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator
 
 import numpy
-import cv2 as cv
-import cv2.aruco as aruco
+import cv2
 
 T0 = numpy.array([0., 0., 0.])
 """Define no translation vector."""
@@ -58,37 +57,16 @@ def is_rotation_matrix(R):
 
     return n < 1e-3
 
-def make_euler_rotation_vector(R):
- 
-    assert(is_rotation_matrix(R))
- 
-    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
- 
-    singular = sy < 1e-6
- 
-    if  not singular :
-        x = math.atan2(R[2,1] , R[2,2])
-        y = math.atan2(-R[2,0], sy)
-        z = math.atan2(R[1,0], R[0,0])
-    else :
-        x = math.atan2(-R[1,2], R[1,1])
-        y = math.atan2(-R[2,0], sy)
-        z = 0
- 
-    return numpy.array([numpy.rad2deg(x), numpy.rad2deg(y), numpy.rad2deg(z)])
-
 @dataclass(frozen=True)
 class Place():
-    """Define a place as a pose and a marker.
+    """Define a place as list of corners position and a marker.
 
     Parameters:
-        translation: position in group referential.
-        rotation: rotation in group referential.
+        corners: 3D corners position in group referential.
         marker: ArUco marker linked to the place.
     """
 
-    translation: numpy.array
-    rotation: numpy.array
+    corners: numpy.array
     marker: dict
 
 @dataclass
@@ -146,12 +124,16 @@ class ArUcoMarkersGroup():
 
                 new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size)
 
-                new_places[identifier] = Place(tvec, rmat, new_marker)
+                # Build marker corners thanks to translation vector and rotation matrix
+                place_corners = numpy.array([[-self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, -self.marker_size/2, 0], [-self.marker_size/2, -self.marker_size/2, 0]])
+                place_corners = place_corners.dot(rmat) + tvec
+
+                new_places[identifier] = Place(place_corners, new_marker)
 
-            # else places are configured using detected markers
+            # else places are configured using detected markers estimated points
             elif isinstance(data, ArUcoMarker.ArUcoMarker):
 
-                new_places[identifier] = Place(data.translation, data.rotation, data)
+                new_places[identifier] = Place(data.points, data)
 
             # else places are already at expected format
             elif (type(identifier) == int) and isinstance(data, Place):
@@ -160,9 +142,6 @@ class ArUcoMarkersGroup():
 
         self.places = new_places
 
-        # Init place consistency
-        self.init_places_consistency()
-
     @classmethod
     def from_obj(self, obj_filepath: str) -> ArUcoMarkersGroupType:
         """Load ArUco markers group from .obj file.
@@ -264,28 +243,16 @@ class ArUcoMarkersGroup():
                 # Retreive marker vertices thanks to face vertice ids
                 for identifier, face in faces.items():
 
-                    # Gather place corners from counter clockwise ordered face vertices
-                    corners = numpy.array([ vertices[i-1] for i in face ])
-
-                    # Edit translation (Tp) allowing to move world axis (W) at place axis (P)
-                    Tp = corners.mean(axis=0)
+                    # Gather place corners in clockwise order
+                    cw_corners = numpy.array([ vertices[i-1] for i in reversed(face) ])
 
                     # Edit place axis from corners positions
-                    place_x_axis = corners[1:3].mean(axis=0) - Tp
+                    place_x_axis = cw_corners[1:3].mean(axis=0)
                     place_x_axis_norm = numpy.linalg.norm(place_x_axis)
-                    place_x_axis = place_x_axis / place_x_axis_norm
-
-                    place_y_axis = corners[2:4].mean(axis=0) - Tp
+  
+                    place_y_axis = cw_corners[2:4].mean(axis=0)
                     place_y_axis_norm = numpy.linalg.norm(place_y_axis)
-                    place_y_axis = place_y_axis / place_y_axis_norm
 
-                    place_z_axis = normals[identifier]
-
-                    # Edit rotation (Rp) allowing to transform world axis (W) into place axis (P)
-                    W = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
-                    P = numpy.array([place_x_axis, place_y_axis, place_z_axis])
-                    Rp = W.dot(P.T)
-                    
                     # Check axis size: they should be almost equal
                     if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3):
 
@@ -302,7 +269,7 @@ class ArUcoMarkersGroup():
 
                     # Create a new place related to a new marker 
                     new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size)
-                    new_places[identifier] = Place(Tp, Rp, new_marker)
+                    new_places[identifier] = Place(cw_corners, new_marker)
 
         except IOError:
             raise IOError(f'File not found: {obj_filepath}')
@@ -337,18 +304,7 @@ class ArUcoMarkersGroup():
         output += '\n\n\tPlaces:'
         for identifier, place in self.places.items():
             output += f'\n\t\t- {identifier}:'
-            output += f'\n{place.translation}'
-            output += f'\n{place.rotation}'
-
-        output += '\n\n\tAngle cache:'
-        for A_identifier, A_angle_cache in self.__rotation_cache.items():
-            for B_identifier, angle in A_angle_cache.items():
-                output += f'\n\t\t- {A_identifier}/{B_identifier}: [{angle[0]:3f} {angle[1]:3f} {angle[2]:3f}]'
-
-        output += '\n\n\tDistance cache:'
-        for A_identifier, A_distance_cache in self.__translation_cache.items():
-            for B_identifier, distance in A_distance_cache.items():
-                output += f'\n\t\t- {A_identifier}/{B_identifier}: {distance:3f}'
+            output += f'\n{place.corners}'
 
         return output
 
@@ -381,148 +337,22 @@ class ArUcoMarkersGroup():
 
         return group_markers, remaining_markers
 
-    def init_places_consistency(self):
-        """Initialize places consistency to speed up further markers consistency checking."""
-
-        # Process expected rotation between places combinations to speed up further calculations
-        self.__rotation_cache = {}
-        for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2):
-
-            A = self.places[A_identifier].rotation
-            B = self.places[B_identifier].rotation
-
-            if numpy.array_equal(A, B):
-
-                AB_rvec = [0., 0., 0.]
-                BA_rvec = [0., 0., 0.]
-
-            else:
-
-                # Calculate euler angle representation of AB and BA rotation matrix
-                AB_rvec = make_euler_rotation_vector(B.dot(A.T))
-                BA_rvec = make_euler_rotation_vector(A.dot(B.T))
-
-            try:
-                self.__rotation_cache[A_identifier][B_identifier] = AB_rvec
-            except:
-                self.__rotation_cache[A_identifier] = {B_identifier: AB_rvec}
-
-            try:
-                self.__rotation_cache[B_identifier][A_identifier] = BA_rvec
-            except:
-                self.__rotation_cache[B_identifier] = {A_identifier: BA_rvec}
-
-        # Process translation between each places combinations to speed up further calculations
-        self.__translation_cache = {}
-        for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2):
-
-            A = self.places[A_identifier].translation
-            B = self.places[B_identifier].translation
-
-            # Calculate translation between A and B position
-            AB_tvec = numpy.linalg.norm(B - A)
-
-            try:
-                self.__translation_cache[A_identifier][B_identifier] = AB_tvec
-            except:
-                self.__translation_cache[A_identifier] = {B_identifier: AB_tvec}
-
-            try:
-                self.__translation_cache[B_identifier][A_identifier] = AB_tvec
-            except:
-                self.__translation_cache[B_identifier] = {A_identifier: AB_tvec}
+    def estimate_pose_from_markers_corners(self, markers: dict, K: numpy.array, D: numpy.array) -> Tuple[bool, numpy.array, numpy.array]:
+        """Estimate pose from markers corners and places corners.
 
-    def check_markers_consistency(self, group_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict, dict]:
-        """Evaluate if given markers configuration match related places configuration.
+        Parameters:
+            markers: detected markers to use for pose estimation.
+            K: intrinsic camera parameters
+            D: camera distorsion matrix
 
         Returns:
-            dict of consistent markers
-            dict of unconsistent markers
-            dict of identified distance or angle unconsistencies and out-of-bounds values
+            success: True if the pose estimation succeeded
+            tvec: scene translation vector
+            rvec: scene rotation vector
         """
 
-        consistent_markers = {}
-        unconsistencies = {'rotation': {}, 'translation': {}}
-
-        for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(group_markers.items(), 2):
-
-            try:
-
-                # Rotation matrix from A marker to B marker
-                AB = B_marker.rotation.dot(A_marker.rotation.T)
-
-                # Calculate euler angle representation of AB rotation matrix
-                AB_rvec = make_euler_rotation_vector(AB)
-                expected_rvec= self.__rotation_cache[A_identifier][B_identifier]
-
-                # Calculate distance between A marker center and B marker center
-                AB_tvec = numpy.linalg.norm(A_marker.translation - B_marker.translation)
-                expected_tvec = self.__translation_cache[A_identifier][B_identifier]
-
-                # Check angle and distance according given tolerance then normalise marker pose
-                consistent_rotation = numpy.allclose(AB_rvec, expected_rvec, atol=angle_tolerance)
-                consistent_translation = math.isclose(AB_tvec, expected_tvec, abs_tol=distance_tolerance)
-
-                if consistent_rotation and consistent_translation:
-
-                    if A_identifier not in consistent_markers.keys():
-
-                        # Remember this marker is already validated
-                        consistent_markers[A_identifier] = A_marker
-
-                    if B_identifier not in consistent_markers.keys():
-
-                        # Remember this marker is already validated
-                        consistent_markers[B_identifier] = B_marker
-
-                else:
-
-                    if not consistent_rotation:
-                        unconsistencies['rotation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_rvec, 'expected': expected_rvec}
-                    
-                    if not consistent_translation:
-                        unconsistencies['translation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_tvec, 'expected': expected_tvec}
-
-            except KeyError:
-
-                raise ValueError(f'Marker {A_identifier} or {B_identifier} don\'t belong to the group.')
-
-        # Gather unconsistent markers
-        unconsistent_markers = {}
-
-        for identifier, marker in group_markers.items():
-
-            if identifier not in consistent_markers.keys():
-
-                unconsistent_markers[identifier] = marker
-
-        return consistent_markers, unconsistent_markers, unconsistencies
-
-    def estimate_pose_from_single_marker(self, marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]:
-        """Calculate rotation and translation that move a marker to its place."""
-
-        # Get the place related to the given marker
-        try:
-
-            place = self.places[marker.identifier]
-
-            # Rotation matrix that transform marker to related place
-            self._rotation = marker.rotation.dot(place.rotation.T)
-
-            # Translation vector that transform marker to related place
-            self._translation = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T)
-
-            return self._translation, self._rotation
-
-        except KeyError:
-
-            raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.')
-
-    def estimate_pose_from_markers(self, markers: dict) -> Tuple[numpy.array, numpy.array]:
-        """Calculate average rotation and translation that move markers to their related places."""
-
-        rotations = []
-        translations = []
+        markers_corners_2d = []
+        places_corners_3d = []
 
         for identifier, marker in markers.items():
 
@@ -530,72 +360,23 @@ class ArUcoMarkersGroup():
 
                 place = self.places[identifier]
 
-                # Rotation matrix that transform marker to related place
-                R = marker.rotation.dot(place.rotation.T)
-
-                # Translation vector that transform marker to related place
-                T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T)
+                for marker_corner in marker.corners:
+                    markers_corners_2d.append(list(marker_corner))
 
-                rotations.append(R)
-                translations.append(T)
+                for place_corner in place.corners:
+                    places_corners_3d.append(list(place_corner))
 
             except KeyError:
 
                 raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.')
 
-        # Consider ArUcoMarkersGroup rotation as the mean of all marker rotations
-        # !!! 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(rotations), axis=0)
-
-        # Consider ArUcoMarkersGroup translation as the mean of all marker translations
-        self._translation = numpy.mean(numpy.array(translations), axis=0)
-
-        return self._translation, self._rotation
-
-    def estimate_pose_from_axis_markers(self, origin_marker: ArUcoMarker.ArUcoMarker, horizontal_axis_marker: ArUcoMarker.ArUcoMarker, vertical_axis_marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]:
-        """Calculate rotation and translation from 3 markers defining an orthogonal axis."""
-
-        O_marker = origin_marker
-        A_marker = horizontal_axis_marker
-        B_marker = vertical_axis_marker
-
-        O_place = self.places[O_marker.identifier]
-        A_place = self.places[A_marker.identifier]
-        B_place = self.places[B_marker.identifier]
-
-        # Place axis
-        OA = A_place.translation - O_place.translation
-        OA = OA / numpy.linalg.norm(OA)
-
-        OB = B_place.translation - O_place.translation
-        OB = OB / numpy.linalg.norm(OB)
-
-        # Detect and correct bad place axis orientation
-        X_sign = numpy.sign(OA)[0]
-        Y_sign = numpy.sign(OB)[1]
-
-        P = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)])
-
-        # Marker axis
-        OA = A_marker.translation - O_marker.translation
-        OA = OA / numpy.linalg.norm(OA)
-
-        OB = B_marker.translation - O_marker.translation
-        OB = OB / numpy.linalg.norm(OB)
-
-        # Detect and correct bad place axis orientation
-        X_sign = numpy.sign(OA)[0]
-        Y_sign = -numpy.sign(OB)[1]
-
-        M = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)])
-
-        # Then estimate ArUcoMarkersGroup rotation
-        self._rotation = P.dot(M.T)
+        # Solve
+        success, rvec, tvec = cv2.solvePnP(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), flags=0)
 
-        # Consider ArUcoMarkersGroup translation as the translation of the marker at axis origin
-        self._translation = O_marker.translation - O_place.translation.dot(O_place.rotation).dot(M.T)
+        self._translation = tvec.T
+        self._rotation = rvec.T
 
-        return self._translation, self._rotation
+        return success, self._translation, self._rotation
 
     @property
     def translation(self) -> numpy.array:
@@ -624,15 +405,15 @@ class ArUcoMarkersGroup():
 
         try:
             axisPoints = numpy.float32([[length, 0, 0], [0, length, 0], [0, 0, length], [0, 0, 0]]).reshape(-1, 3)
-            axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
+            axisPoints, _ = cv2.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
             axisPoints = axisPoints.astype(int)
 
-            cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red)
-            cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green)
-            cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue)
+            cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red)
+            cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green)
+            cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue)
 
         # Ignore errors due to out of field axis: their coordinate are larger than int32 limitations.
-        except cv.error:
+        except cv2.error:
             pass
 
     def draw_places(self, image: numpy.array, K, D, color: tuple = None, border_size: int = 0):
@@ -648,16 +429,16 @@ class ArUcoMarkersGroup():
                 R = self.places[identifier].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))
+                placePoints, _ = cv2.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
                 placePoints = placePoints.astype(int)
                 
-                cv.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), color, border_size)
-                cv.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), color, border_size)
-                cv.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), color, border_size)
-                cv.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), color, border_size)
+                cv2.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), color, border_size)
+                cv2.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), color, border_size)
+                cv2.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), color, border_size)
+                cv2.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), color, border_size)
 
             # Ignore errors due to out of field places: their coordinate are larger than int32 limitations.
-            except cv.error:
+            except cv2.error:
                 pass
 
     def draw_places_axes(self, image: numpy.array, K, D, thickness: int = 0, length: float = 0):
@@ -671,15 +452,15 @@ class ArUcoMarkersGroup():
                 R = self.places[identifier].rotation
 
                 axisPoints = (T + numpy.float32([R.dot([length, 0, 0]), R.dot([0, length, 0]), R.dot([0, 0, length]), R.dot([0, 0, 0])])).reshape(-1, 3)
-                axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
+                axisPoints, _ = cv2.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
                 axisPoints = axisPoints.astype(int)
 
-                cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red)
-                cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green)
-                cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue)
+                cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red)
+                cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green)
+                cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue)
                 
             # Ignore errors due to out of field places: their coordinate are larger than int32 limitations.
-            except cv.error:
+            except cv2.error:
                 pass
 
     def draw(self, image: numpy.array, K, D, draw_axes: dict = None, draw_places: dict = None, draw_places_axes: dict = None):
diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index f6b303a..b8b9cfd 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -96,14 +96,13 @@ class ArUcoScene(ArFeatures.ArScene):
 		# Create new aruco scene using temporary ar scene values
 		return ArUcoScene(aruco_markers_group=new_aruco_markers_group, **temp_ar_scene_values)
 	
-	def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, str, dict]:
+	def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, dict]:
 		"""Estimate scene pose from detected ArUco markers.
 
 		Returns:
-				scene translation vector
-				scene rotation matrix
-				pose estimation strategy
-				dict of markers used to estimate the pose
+			scene translation vector
+			scene rotation matrix
+			dict of markers used to estimate the pose
 		"""
 
 		# Pose estimation fails when no marker is detected
@@ -118,26 +117,14 @@ class ArUcoScene(ArFeatures.ArScene):
 
 			raise ArFeatures.PoseEstimationFailed('No marker belongs to the scene')
 
-		# Estimate scene pose from unique marker transformations
-		elif len(scene_markers) == 1:
+		# Estimate pose from a markers corners
+		success, tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers_corners(scene_markers, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D)
 
-			marker_id, marker = scene_markers.popitem()
-			tvec, rmat = self.aruco_markers_group.estimate_pose_from_single_marker(marker)
-			
-			return tvec, rmat, 'estimate_pose_from_single_marker', {marker_id: marker}
+		if not success:
 
-		# Otherwise, check markers consistency
-		consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.check_markers_consistency(scene_markers, self.angle_tolerance, self.distance_tolerance)
+			raise ArFeatures.PoseEstimationFailed('Can\'t estimate pose from markers corners positions')
 
-		# Pose estimation fails when no marker passes consistency checking
-		if len(consistent_markers) == 0:
-
-			raise ArFeatures.PoseEstimationFailed('Unconsistent marker poses', unconsistencies)
-
-		# Otherwise, estimate scene pose from all consistent markers pose
-		tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers(consistent_markers)
-
-		return tvec, rmat, 'estimate_pose_from_markers', consistent_markers
+		return tvec, rmat, scene_markers
 
 	def draw(self, image: numpy.array, draw_aruco_markers_group: dict = None):
 		"""