From 57b54b559ab312a764d0dca294140bc5073abd2d Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Tue, 25 Apr 2023 17:50:18 +0200
Subject: Fixing scene translation estimation based on axis marker.

---
 src/argaze/ArUcoMarkers/ArUcoScene.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 70ce236..1bb76cd 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -527,6 +527,7 @@ class ArUcoScene():
 
         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)
 
@@ -543,7 +544,7 @@ class ArUcoScene():
         self._rotation = P.dot(M.T)
 
         # Consider ArUcoScene translation as the translation of the marker at axis origin
-        self._translation = O_marker.translation - O_place.translation.dot(O_place.rotation).dot(O_marker.rotation.T)
+        self._translation = O_marker.translation - O_place.translation.dot(O_place.rotation).dot(M.T)
 
         return self._translation, self._rotation
 
-- 
cgit v1.1