diff options
| -rw-r--r-- | src/argaze/utils/environment_edit.py | 97 |
1 files changed, 81 insertions, 16 deletions
diff --git a/src/argaze/utils/environment_edit.py b/src/argaze/utils/environment_edit.py index 36bd0a2..40ba86a 100644 --- a/src/argaze/utils/environment_edit.py +++ b/src/argaze/utils/environment_edit.py @@ -118,7 +118,7 @@ def main(): gaze_position = GazeFeatures.GazePosition(pointer, precision=2) # Reset info frame - info_frame = numpy.full((800, 1000, 3), 0, dtype=numpy.uint8) + info_frame = numpy.full((850, 1500, 3), 0, dtype=numpy.uint8) # Select a new frame and detect markers once if next_frame_index != current_frame_index or refresh or draw_cover: @@ -219,19 +219,20 @@ def main(): selected_marker = ar_environment.aruco_detector.detected_markers[selected_marker_id] # Write selected marker id - cv2.rectangle(info_frame, (0, 0), (1000, 50), selected_marker.color, -1) + cv2.rectangle(info_frame, (0, 0), (500, 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) + cv2.rectangle(info_frame, (0, 50), (500, frame_height), (255, 255, 255), -1) # Write selected marker rotation matrix 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'Rotation (camera axis)', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 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 - cv2.putText(info_frame, f'Translation vector:', (20, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'Translation (camera axis):', (20, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) cv2.putText(info_frame, f'{T[0]:.3f}', (40, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) cv2.putText(info_frame, f'{T[1]:.3f}', (40, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) cv2.putText(info_frame, f'{T[2]:.3f}', (40, 440), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) @@ -239,8 +240,6 @@ def main(): # Retreive selected marker place selected_place = ar_scene.aruco_scene.places[selected_marker_id] - cv2.putText(info_frame, f'Edited place #{selected_place.marker.identifier}', (520, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) - # On right click if right_button: @@ -281,20 +280,86 @@ def main(): # Retreive hovered marker hovered_marker = ar_environment.aruco_detector.detected_markers[hovered_marker_id] + # Write hovered marker id + cv2.rectangle(info_frame, (500, 0), (1000, 50), hovered_marker.color, -1) + cv2.putText(info_frame, f'Hovered marker #{hovered_marker.identifier}', (520, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.rectangle(info_frame, (500, 50), (1000, frame_height), (255, 255, 255), -1) + + # Write hovered marker rotation matrix + R = ArUcoScene.make_euler_rotation_vector(hovered_marker.rotation) + cv2.putText(info_frame, f'Rotation (camera axis)', (520, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 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 hovered marker translation vector + T = hovered_marker.translation + cv2.putText(info_frame, f'Translation (camera axis):', (520, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[0]:.3f}', (540, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[1]:.3f}', (540, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[2]:.3f}', (540, 440), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + # Retreive hovered marker place hovered_place = ar_scene.aruco_scene.places[hovered_marker_id] - # Write rotation info between markers and places + # Write hovered place rotation matrix + R = ArUcoScene.make_euler_rotation_vector(hovered_place.rotation) + cv2.putText(info_frame, f'Rotation (scene axis):', (520, 500), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[0]:.3f}', (540, 540), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[1]:.3f}', (540, 580), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[2]:.3f}', (540, 620), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + + # Write hovered place translation vector + T = hovered_place.translation + cv2.putText(info_frame, f'Translation (scene axis):', (520, 700), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[0]:.3f}', (540, 740), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[1]:.3f}', (540, 780), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[2]:.3f}', (540, 820), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + + # Rotation 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) - # Write translation info between markers and places + # Translation info between markers and places markers_translation = hovered_marker.translation - selected_marker.translation places_translation = hovered_place.translation - selected_place.translation + # Write selected/hovered markers id + cv2.rectangle(info_frame, (1000, 0), (1500, 50), (63, 63, 63), -1) + cv2.putText(info_frame, f'#{selected_marker.identifier} -> #{hovered_marker.identifier}', (1020, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) + cv2.rectangle(info_frame, (1000, 50), (1500, frame_height), (190, 190, 190), -1) + + # Write selected/hovered markers rotation matrix + R = markers_rotation_vector + cv2.putText(info_frame, f'Rotation (camera axis)', (1020, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[0]:.3f}', (1040, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[1]:.3f}', (1040, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[2]:.3f}', (1040, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + + # Write selected/hovered markers translation vector + T = markers_translation + cv2.putText(info_frame, f'Translation (camera axis):', (1020, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[0]:.3f}', (1040, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[1]:.3f}', (1040, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[2]:.3f}', (1040, 440), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + + # Write selected/hovered places rotation matrix + R = places_rotation_vector + cv2.putText(info_frame, f'Rotation (scene axis):', (1020, 500), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[0]:.3f}', (1040, 540), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[1]:.3f}', (1040, 580), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[2]:.3f}', (1040, 620), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + + # Write selected/hovered places translation vector + T = places_translation + cv2.putText(info_frame, f'Translation (scene axis):', (1020, 700), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[0]:.3f}', (1040, 740), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[1]:.3f}', (1040, 780), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[2]:.3f}', (1040, 820), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + if snap: # Snap once @@ -328,17 +393,17 @@ def main(): # Write edited place rotation matrix 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) + cv2.putText(info_frame, f'Rotation (scene axis):', (20, 500), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[0]:.3f}', (40, 540), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[1]:.3f}', (40, 580), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{R[2]:.3f}', (40, 620), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) # Write edited place translation vector T = edited_place.translation - cv2.putText(info_frame, f'Translation vector:', (520, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(info_frame, f'{T[0]:.3f}', (540, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) - cv2.putText(info_frame, f'{T[1]:.3f}', (540, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) - cv2.putText(info_frame, f'{T[2]:.3f}', (540, 440), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'Translation (scene axis):', (20, 700), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[0]:.3f}', (40, 740), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[1]:.3f}', (40, 780), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA) + cv2.putText(info_frame, f'{T[2]:.3f}', (40, 820), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA) # Replace selected place by edited place ar_scene.aruco_scene.places[selected_marker_id] = edited_place |