aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/utils/environment_edit.py95
1 files changed, 46 insertions, 49 deletions
diff --git a/src/argaze/utils/environment_edit.py b/src/argaze/utils/environment_edit.py
index 6bf252b..7d5411b 100644
--- a/src/argaze/utils/environment_edit.py
+++ b/src/argaze/utils/environment_edit.py
@@ -144,6 +144,9 @@ def main():
# Detect markers
ar_environment.aruco_detector.detect_markers(video_frame)
+ # Estimate all marker's pose
+ ar_environment.aruco_detector.estimate_markers_pose()
+
# Write detected markers
cv2.putText(video_frame, f'{list(ar_environment.aruco_detector.detected_markers.keys())}', (20, frame_height-80), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
@@ -219,15 +222,12 @@ def main():
cv2.rectangle(info_frame, (0, 0), (1000, 50), selected_marker.color, -1)
cv2.putText(info_frame, f'Selected marker #{selected_marker.identifier}', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- # Estimate selected marker pose
- ar_environment.aruco_detector.estimate_markers_pose([selected_marker_id])
-
# Write selected marker rotation matrix
- R = selected_marker.rotation
- cv2.putText(info_frame, f'Rotation matrix:', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(info_frame, f'{R[0][0]:.3f} {R[0][1]:.3f} {R[0][2]:.3f}', (40, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_frame, f'{R[1][0]:.3f} {R[1][1]:.3f} {R[1][2]:.3f}', (40, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_frame, f'{R[2][0]:.3f} {R[2][1]:.3f} {R[2][2]:.3f}', (40, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA)
+ R = ArUcoScene.make_euler_rotation_vector(selected_marker.rotation)
+ cv2.putText(info_frame, f'Rotation vector:', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+ cv2.putText(info_frame, f'{R[0]:.3f}', (40, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
+ cv2.putText(info_frame, f'{R[1]:.3f}', (40, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
+ cv2.putText(info_frame, f'{R[2]:.3f}', (40, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA)
# Write selected marker translation vector
T = selected_marker.translation
@@ -275,11 +275,8 @@ def main():
edited_place = selected_place
- # Snap key pressed when another marker is hovered
- if snap and hovered_marker_id >= 0 and hovered_marker_id != selected_marker_id:
-
- # Snap once
- snap = False
+ # A marker is hovered while another is selected
+ if hovered_marker_id >= 0 and hovered_marker_id != selected_marker_id:
# Retreive hovered marker
hovered_marker = ar_environment.aruco_detector.detected_markers[hovered_marker_id]
@@ -287,51 +284,54 @@ def main():
# Retreive hovered marker place
hovered_place = ar_scene.aruco_scene.places[hovered_marker_id]
- # Estimate hovered marker pose
- ar_environment.aruco_detector.estimate_markers_pose([hovered_marker_id])
-
- # Estimate pose from hovered marker
- tvec, rmat = ar_scene.aruco_scene.estimate_pose_from_single_marker(hovered_marker)
-
- print(f'******* SNAP {selected_marker_id} / {hovered_marker_id} *******')
- '''
- # Edit rotation transformation
+ # Write rotation info between markers and places
markers_rotation_matrix = hovered_marker.rotation.dot(selected_marker.rotation.T)
places_rotation_matrix = hovered_place.rotation.dot(selected_place.rotation.T)
markers_rotation_vector = ArUcoScene.make_euler_rotation_vector(markers_rotation_matrix)
places_rotation_vector = ArUcoScene.make_euler_rotation_vector(places_rotation_matrix)
- #R = places_rotation_matrix.dot(rmat.T).dot(markers_rotation_matrix.T).dot(rmat)
-
- #rmat_places_rotation_vector = ArUcoScene.make_euler_rotation_vector(places_rotation_matrix.dot(rmat.T))
- rdiff = places_rotation_vector - markers_rotation_vector
- R = ArUcoScene.make_rotation_matrix(*rdiff)
-
- print(f'markers_rotation_vector: {markers_rotation_vector}')
- print(f'places_rotation_vector: {places_rotation_vector}')
- print(f'rdiff: {rdiff}')
- print(f'R: {ArUcoScene.make_euler_rotation_vector(R)}')
- '''
- # Edit translation transformation
+ # Write translation info between markers and places
markers_translation = hovered_marker.translation - selected_marker.translation
places_translation = hovered_place.translation - selected_place.translation
- T = (places_translation.dot(rmat.T) - markers_translation).dot(rmat)
+ if snap:
- print(f'markers_translation: {markers_translation} ({numpy.linalg.norm(markers_translation)})')
- print(f'places_translation: {places_translation} ({numpy.linalg.norm(places_translation)})')
- print(f'T: {T} ({numpy.linalg.norm(T)})')
+ # Snap once
+ snap = False
- # Apply transformations
- edited_place = ArUcoScene.Place(selected_place.translation + T, selected_place.rotation, selected_marker)
+ print(f'******* SNAP {selected_marker_id} / {hovered_marker_id} *******')
+
+ '''
+ # Edit rotation transformation
+ #R = places_rotation_matrix.dot(rmat.T).dot(markers_rotation_matrix.T).dot(rmat)
+
+ #rmat_places_rotation_vector = ArUcoScene.make_euler_rotation_vector(places_rotation_matrix.dot(rmat.T))
+ rdiff = places_rotation_vector - markers_rotation_vector
+ R = ArUcoScene.make_rotation_matrix(*rdiff)
+
+ print(f'markers_rotation_vector: {markers_rotation_vector}')
+ print(f'places_rotation_vector: {places_rotation_vector}')
+ print(f'rdiff: {rdiff}')
+ print(f'R: {ArUcoScene.make_euler_rotation_vector(R)}')
+ '''
+
+ # Edit translation transformation
+ T = (places_translation.dot(rmat.T) - markers_translation).dot(rmat)
+
+ print(f'markers_translation: {markers_translation} ({numpy.linalg.norm(markers_translation)})')
+ print(f'places_translation: {places_translation} ({numpy.linalg.norm(places_translation)})')
+ print(f'T: {T} ({numpy.linalg.norm(T)})')
+
+ # Apply transformations
+ edited_place = ArUcoScene.Place(selected_place.translation + T, selected_place.rotation, selected_marker)
# Write edited place rotation matrix
- R = edited_place.rotation
- cv2.putText(info_frame, f'Rotation matrix:', (520, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(info_frame, f'{R[0][0]:.3f} {R[0][1]:.3f} {R[0][2]:.3f}', (540, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_frame, f'{R[1][0]:.3f} {R[1][1]:.3f} {R[1][2]:.3f}', (540, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_frame, f'{R[2][0]:.3f} {R[2][1]:.3f} {R[2][2]:.3f}', (540, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA)
+ R = ArUcoScene.make_euler_rotation_vector(edited_place.rotation)
+ cv2.putText(info_frame, f'Rotation vector:', (520, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+ cv2.putText(info_frame, f'{R[0]:.3f}', (540, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
+ cv2.putText(info_frame, f'{R[1]:.3f}', (540, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
+ cv2.putText(info_frame, f'{R[2]:.3f}', (540, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA)
# Write edited place translation vector
T = edited_place.translation
@@ -348,7 +348,7 @@ def main():
# Estimate scene pose from each marker
cv2.putText(video_frame, f'Single marker scene pose estimation', (20, frame_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
+
for i, m in ar_environment.aruco_detector.detected_markers.items():
tvec, rmat = ar_scene.aruco_scene.estimate_pose_from_single_marker(m)
@@ -379,9 +379,6 @@ def main():
cv2.putText(info_frame, f'Left click on marker to select it', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- # Estimate all marker's pose
- ar_environment.aruco_detector.estimate_markers_pose()
-
if len(ar_environment.aruco_detector.detected_markers) > 1:
# Check markers consistency