Estimating marker poses once. Writing rotation vector.

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