From 61634d1e0e133fd61571d405425db7e3b56f9137 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 26 Apr 2023 23:06:18 +0200 Subject: Easing place pose edition. --- src/argaze/utils/environment_edit.py | 94 ++++++++++++++++++++++++------------ 1 file changed, 62 insertions(+), 32 deletions(-) (limited to 'src') 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' -- cgit v1.1