Writing hovered marker and place pose info.

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