diff options
| -rw-r--r-- | src/argaze/utils/environment_edit.py | 95 |
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 |