aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py61
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)