From 3934f14dbab4206d091f07dd29bf3ad3dfb9b787 Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Tue, 17 Oct 2023 09:14:11 +0200
Subject: Adding useAruco3Detection as possible parameters.

---
 src/argaze/ArUcoMarkers/ArUcoDetector.py     |  3 ++-
 src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py | 11 +++++++++--
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/argaze/ArUcoMarkers/ArUcoDetector.py b/src/argaze/ArUcoMarkers/ArUcoDetector.py
index 51f8366..e62a42e 100644
--- a/src/argaze/ArUcoMarkers/ArUcoDetector.py
+++ b/src/argaze/ArUcoMarkers/ArUcoDetector.py
@@ -72,7 +72,8 @@ class DetectorParameters():
         'minOtsuStdDev',
         'perspectiveRemoveIgnoredMarginPerCell',
         'perspectiveRemovePixelPerCell',
-        'polygonalApproxAccuracyRate'
+        'polygonalApproxAccuracyRate',
+        'useAruco3Detection'
     ]
 
     def __init__(self, **kwargs):
diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
index dddead4..8600690 100644
--- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
+++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
@@ -370,8 +370,15 @@ class ArUcoMarkersGroup():
 
                 raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.')
 
-        # Solve
-        success, rvec, tvec = cv2.solvePnP(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), flags=3)
+        # SolvPnP using cv2.SOLVEPNP_SQPNP flag
+        # TODO: it works also with cv2.SOLVEPNP_EPNP flag so we need to test which is the faster.
+        # About SolvPnP flags: https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html
+        success, rvec, tvec = cv2.solvePnP(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), flags=cv2.SOLVEPNP_SQPNP)
+
+        # Refine pose estimation using Gauss-Newton optimisation
+        #if success :
+
+            #rvec, tvec = cv2.solvePnPRefineVVS(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), rvec, tvec)
 
         self._translation = tvec.T
         self._rotation = rvec.T
-- 
cgit v1.1