diff options
| -rw-r--r-- | src/argaze/utils/environment_edit.py | 67 |
1 files changed, 62 insertions, 5 deletions
diff --git a/src/argaze/utils/environment_edit.py b/src/argaze/utils/environment_edit.py index aa208e5..6bf252b 100644 --- a/src/argaze/utils/environment_edit.py +++ b/src/argaze/utils/environment_edit.py @@ -47,6 +47,7 @@ def main(): right_button = False edit_trans = False # translate edit_z = False + snap = False draw_help = False draw_grid = False draw_cover = False @@ -183,7 +184,7 @@ def main(): # Hover by pointing on marker if marker_aoi.contains_point(pointer): - + hovered_marker_id = marker_id # Edit marker's color @@ -263,15 +264,67 @@ def main(): else: place_edit[selected_marker_id]['translation'] = (-pointer_delta_x, pointer_delta_y, 0) - # Apply transformations - R = selected_place.rotation.dot(ArUcoScene.make_rotation_matrix(*place_edit[selected_marker_id]['rotation']).T) - T = selected_place.translation + numpy.array(place_edit[selected_marker_id]['translation']) + # Edit transformations + R = ArUcoScene.make_rotation_matrix(*place_edit[selected_marker_id]['rotation']).T + T = numpy.array(place_edit[selected_marker_id]['translation']) - edited_place = ArUcoScene.Place(T, R, selected_marker) + # Apply transformations + edited_place = ArUcoScene.Place(selected_place.translation + T, selected_place.rotation.dot(R), selected_marker) else: 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 + + # Retreive hovered marker + hovered_marker = ar_environment.aruco_detector.detected_markers[hovered_marker_id] + + # 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 + 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 + 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) + + 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 @@ -521,6 +574,10 @@ def main(): if key_pressed == 122: edit_z = not edit_z + # Snap hovered marker with s key + if key_pressed == 115: + snap = True + # Switch help mode with h key if key_pressed == 104: draw_help = not draw_help |