diff options
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 61 |
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) |