Easing place pose edition.

1 files changed, 62 insertions, 32 deletions

diff --git a/src/argaze/utils/environment_edit.py b/src/argaze/utils/environment_edit.py
index dd53df2..1edd623 100644
--- a/src/argaze/utils/environment_edit.py
+++ b/src/argaze/utils/environment_edit.py
@@ -133,15 +133,6 @@ def main():
                 current_frame_index = video_capture.get(cv2.CAP_PROP_POS_FRAMES) - 1
                 current_frame_time = video_capture.get(cv2.CAP_PROP_POS_MSEC)
 
-                # Draw gray panel on top
-                cv2.rectangle(video_frame, (0, 0), (frame_width, 50), (63, 63, 63), -1)
-
-                # Draw camera calibration
-                if draw_grid:
-
-                    cv2.putText(video_frame, f'Grid at {z_grid} cm', (500, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-                    ar_environment.aruco_detector.camera.draw(video_frame, frame_width/10, frame_height/10, z_grid, color=(127, 127, 127))
-
                 # Hide zone
                 if draw_cover:
 
@@ -151,18 +142,16 @@ def main():
                 # Detect markers
                 ar_environment.aruco_detector.detect_markers(video_frame)
 
-                # Edit marker's color
-                for i, m in ar_environment.aruco_detector.detected_markers.items():
-
-                    color_list = list(itertools.permutations([0, 255, 255]))
+                # Draw gray panel on top
+                cv2.rectangle(video_frame, (0, 0), (frame_width, 50), (63, 63, 63), -1)
 
-                    m.color = color_list[i%len(color_list)]
+                # Draw camera calibration
+                if draw_grid:
 
-                    # Draw center
-                    cv2.circle(video_frame, m.center.astype(int), 5, m.color, -1)
+                    cv2.putText(video_frame, f'Grid at {z_grid} cm', (500, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+                    ar_environment.aruco_detector.camera.draw(video_frame, frame_width/10, frame_height/10, z_grid, color=(127, 127, 127))
 
                 # Write timing
-                
                 cv2.putText(video_frame, f'Time: {int(current_frame_time)} ms', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
                 
                 # Copy frame
@@ -190,6 +179,19 @@ def main():
 
                     selected_marker_id = -1
 
+            # Edit marker's color
+            color_list = list(itertools.permutations([0, 255, 255]))
+
+            for i, m in ar_environment.aruco_detector.detected_markers.items():
+
+                m.color = color_list[i%len(color_list)]
+
+                if selected_marker_id >= 0 and i != selected_marker_id:
+                    m.color = (127, 127, 127)
+
+                # Draw center
+                cv2.circle(video_frame, m.center.astype(int), 5, m.color, -1)
+
             try:
 
                 # A marker is selected
@@ -276,21 +278,31 @@ def main():
                         # Replace selected place by edited place
                         ar_scene.aruco_scene.places[selected_marker_id] = edited_place
 
-                        # Estimate scene pose considering only selected marker
-                        tvec, rmat, strategy, _ = ar_scene.estimate_pose({selected_marker_id: selected_marker})
-                        strategy = strategy.replace('_', ' ')
+                        # Refresh places consitency
+                        ar_scene.aruco_scene.init_places_consistency()
 
-                        # Write pose estimation strategy
-                        cv2.putText(video_frame, f'{strategy} with marker {selected_marker_id}', (20, frame_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-            
-                        # Draw expected marker places
-                        ar_scene.draw_places(video_frame)
+                        # 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():
 
-                        # Project AOI scene into frame according estimated pose
-                        aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV)
+                            tvec, rmat = ar_scene.aruco_scene.estimate_pose_from_single_marker(m)
+
+                            # Project AOI scene into frame according estimated pose
+                            aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV)
+
+                            if i == selected_marker_id:
 
-                        # Draw AOI scene projection with gaze
-                        aoi_scene_projection.draw_circlecast(video_frame, gaze_position, base_color=selected_marker.color, looked_color=selected_marker.color)
+                                # Draw AOI scene projection with gaze
+                                aoi_scene_projection.draw_circlecast(video_frame, gaze_position, base_color=m.color, looked_color=(255, 255, 255))
+
+                            else:
+
+                                # Draw AOI scene
+                                aoi_scene_projection.draw(video_frame, color=m.color)
+
+                        # Draw expected marker places
+                        ar_scene.draw_places(video_frame)
 
                     # Catch missing selected marker
                     except KeyError:
@@ -319,6 +331,11 @@ def main():
                         for i, (label, value) in enumerate(unconsistencies.items()):
                             cv2.putText(info_frame, f'Unconsistent {label}: {value:.3f}', (20, 120+ i*40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
      
+                    # Force pose mode to single marker scene pose estimation
+                    else: 
+
+                        pose_mode = 0
+
                     # Single marker scene pose estimation
                     if pose_mode == 0:
 
@@ -334,8 +351,21 @@ def main():
                             # Draw AOI scene
                             aoi_scene_projection.draw(video_frame, color=m.color)
 
+                    # Consistent markers scene pose estimation
+                    if pose_mode == 1:
+
+                        cv2.putText(video_frame, f'Consistent markers scene pose estimation', (20, frame_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+                
+                        tvec, rmat = ar_scene.aruco_scene.estimate_pose_from_markers(consistent_markers)
+
+                        # Project AOI scene into frame according estimated pose
+                        aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV)
+
+                        # Draw AOI scene
+                        aoi_scene_projection.draw(video_frame, color=(255, 255, 255))
+
                     # ArUco marker axis scene pose estimation
-                    elif pose_mode == 1:
+                    elif pose_mode == 2:
 
                         # Write pose estimation strategy
                         cv2.putText(video_frame, f'ArUco marker axis scene pose estimation', (20, frame_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
@@ -362,7 +392,7 @@ def main():
                                 pass
 
                     # ArUco AOI scene building
-                    elif pose_mode == 2:
+                    elif pose_mode == 3:
 
                         # Write pose estimation strategy
                         cv2.putText(video_frame, f'ArUco AOI scene building', (20, frame_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
@@ -482,7 +512,7 @@ def main():
         # Switch pose estimation mode with m key
         if key_pressed == 109:
             pose_mode += 1
-            if pose_mode > 2:
+            if pose_mode > 3:
                 pose_mode = 0
 
         # Save selected marker edition using 'Ctrl + s'