aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/utils/environment_edit.py67
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