Fixing marker pose normalisation. Drawing place axis.

1 files changed, 36 insertions, 25 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index a0b2a89..06d63d3 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -274,26 +274,24 @@ class ArUcoScene():
                     # Gather place corners from counter clockwise ordered face vertices
                     corners = numpy.array([ vertices[i-1] for i in face ])
 
-                    # Edit place translation as corners center
-                    T = corners.mean(axis=0)
+                    # Edit translation (Tp) allowing to move world axis (W) at place axis (P)
+                    Tp = corners.mean(axis=0)
 
-                    # Edit place rotation from corners positions
-                    marker_x_axis = corners[1:3].mean(axis=0) - T
-                    marker_x_axis = marker_x_axis / numpy.linalg.norm(marker_x_axis)
+                    # Edit place axis from corners positions
+                    place_x_axis = corners[1:3].mean(axis=0) - Tp
+                    place_x_axis = place_x_axis / numpy.linalg.norm(place_x_axis)
 
-                    marker_y_axis = corners[2:4].mean(axis=0) - T
-                    marker_y_axis = marker_y_axis / numpy.linalg.norm(marker_y_axis)
+                    place_y_axis = corners[2:4].mean(axis=0) - Tp
+                    place_y_axis = place_y_axis / numpy.linalg.norm(place_y_axis)
 
-                    marker_z_axis = normals[name]
+                    place_z_axis = normals[name]
 
-                    # RM = P
-                    M = numpy.array([marker_x_axis, marker_y_axis, marker_z_axis])
-                    P = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
-                    R = P.dot(M.T)
+                    # 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)
 
-                    marker = markers[name]
-
-                    self.__places[name] = Place(T, R, marker)
+                    self.__places[name] = Place(Tp, Rp, markers[name])
 
         except IOError:
             raise IOError(f'File not found: {obj_filepath}')
@@ -301,10 +299,13 @@ class ArUcoScene():
     def __normalise_marker_pose(self, place, R, T):
 
         # Transform marker rotation into scene rotation matrix
-        Rs = place.rotation.dot(R)
+        Rs = R.dot(place.rotation.T)
 
         # Transform marker translation into scene translation vector
-        Ts = T + place.rotation.dot(R.dot(-place.translation))
+        #Ts = T + place.rotation.dot(R.dot(-place.translation))
+        #Ts = T - place.rotation.dot(R.dot(place.translation))
+        #Ts = T - place.translation.dot(place.rotation.T)
+        Ts = T - place.translation.dot(place.rotation).dot(R.T)
 
         return Rs, Ts
 
@@ -508,12 +509,12 @@ class ArUcoScene():
         axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
         axisPoints = axisPoints.astype(int)
 
-        cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 5) # X (red)
-        cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 5) # Y (green)
-        cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 5) # Z (blue)
+        cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 6) # X (red)
+        cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green)
+        cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue)
 
     def draw_places(self, frame, K, D):
-        """Draw scene places."""
+        """Draw scene places and their axis."""
 
         l = self.__marker_size / 2
         ll = self.__marker_size
@@ -527,11 +528,21 @@ class ArUcoScene():
             T = self.__places[name].translation
             R = self.__places[name].rotation
 
+            # Draw place axis
+            axisPoints = (T + numpy.float32([R.dot([l/2, 0, 0]), R.dot([0, l/2, 0]), R.dot([0, 0, l/2]), R.dot([0, 0, 0])])).reshape(-1, 3)
+            axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
+            axisPoints = axisPoints.astype(int)
+
+            cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 6) # X (red)
+            cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green)
+            cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue)
+
+            # Draw place
             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 = placePoints.astype(int)
             
-            cv.line(frame, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), (f,f,f), 2)
-            cv.line(frame, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), (f,f,f), 2)
-            cv.line(frame, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), (f,f,f), 2)
-            cv.line(frame, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), (f,f,f), 2)
+            cv.line(frame, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), (f,f,f), 3)
+            cv.line(frame, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), (f,f,f), 3)
+            cv.line(frame, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), (f,f,f), 3)
+            cv.line(frame, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), (f,f,f), 3)