Snapping a place position using another marker position.

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