aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/utils/demo_heatmap_run.py46
-rw-r--r--src/argaze/utils/environment_edit.py712
2 files changed, 0 insertions, 758 deletions
diff --git a/src/argaze/utils/demo_heatmap_run.py b/src/argaze/utils/demo_heatmap_run.py
deleted file mode 100644
index be31d31..0000000
--- a/src/argaze/utils/demo_heatmap_run.py
+++ /dev/null
@@ -1,46 +0,0 @@
-#!/usr/bin/env python
-
-import os
-
-from argaze.AreaOfInterest import AOIFeatures
-
-import numpy
-import cv2
-
-def main():
-
- window_name = 'Heatmap'
- image_size = (800, 600)
-
- heatmap = AOIFeatures.Heatmap(image_size)
-
- heatmap.init()
-
- cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE)
-
- # Update pointer position
- def on_mouse_event(event, x, y, flags, param):
-
- heatmap.update((x, y), sigma=0.05)
-
- # Attach mouse callback to window
- cv2.setMouseCallback(window_name, on_mouse_event)
-
- while True:
-
- cv2.imshow(window_name, heatmap.image)
-
- # Stop and save picture by pressing 'Esc' key
- if cv2.waitKey(10) == 27:
-
- current_directory = os.path.dirname(os.path.abspath(__file__))
- cv2.imwrite(os.path.join(current_directory,'heatmap.png'), heatmap.image)
-
- break
-
- # Stop image display
- cv2.destroyAllWindows()
-
-if __name__ == '__main__':
-
- main() \ No newline at end of file
diff --git a/src/argaze/utils/environment_edit.py b/src/argaze/utils/environment_edit.py
deleted file mode 100644
index 1cf1d2f..0000000
--- a/src/argaze/utils/environment_edit.py
+++ /dev/null
@@ -1,712 +0,0 @@
-#!/usr/bin/env python
-
-""" """
-
-__author__ = "Théo de la Hogue"
-__credits__ = []
-__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)"
-__license__ = "BSD"
-
-import argparse
-import time
-import itertools
-
-from argaze import ArFeatures, GazeFeatures
-from argaze.AreaOfInterest import AOIFeatures
-from argaze.ArUcoMarkers import ArUcoScene
-from argaze.utils import MiscFeatures
-
-from tobiiproglasses2 import *
-
-import cv2
-import numpy
-
-def main():
- """
- Load AR environment from .json file, detect ArUco markers into movie images and estimate environment pose.
- Edit environment setup to improve pose estimation.
- """
-
- # Manage arguments
- parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
- parser.add_argument('environment', metavar='ENVIRONMENT', type=str, help='ar environment filepath')
- parser.add_argument('movie', metavar='MOVIE', type=str, default=None, help='movie path')
- parser.add_argument('-s','--start', metavar='START', type=float, default=0., help='start time in second')
- parser.add_argument('-o', '--output', metavar='OUT', type=str, default='edited_environment.json', help='edited ar environment file path')
- args = parser.parse_args()
-
- # Load AR enviroment
- ar_environment = ArFeatures.ArEnvironment.from_json(args.environment)
-
- #print('ArEnvironment:\n', ar_environment)
-
- # Select first AR scene
- ar_scene = list(ar_environment.scenes.values())[0]
-
- # Create a window to display AR environment
- cv2.namedWindow(ar_environment.name, cv2.WINDOW_AUTOSIZE)
-
- # Init mouse interaction
- pointer = (0, 0)
- left_click = (0, 0)
- right_click = (0, 0)
- right_drag = (0, 0)
- right_button = False
- edit_trans = False # translate
- edit_z = False
- snap = False
- draw_help = False
- draw_grid = False
- draw_cover = False
- pose_mode = 0
- z_grid = 100.
-
- # Update pointer position
- def on_mouse_event(event, x, y, flags, param):
-
- nonlocal pointer
- nonlocal left_click
- nonlocal right_click
- nonlocal right_drag
- nonlocal right_button
-
- # Update pointer
- pointer = (x, y)
-
- # Update left_click
- if event == cv2.EVENT_LBUTTONUP:
-
- left_click = pointer
-
- # Udpate right_button
- elif event == cv2.EVENT_RBUTTONDOWN and not right_button:
-
- right_button = True
- right_click = pointer
-
- elif event == cv2.EVENT_RBUTTONUP and right_button:
-
- right_button = False
-
- # Udpate right_drag
- if right_button:
-
- right_drag = (pointer[0] - right_click[0], pointer[1] - right_click[1])
-
- # Attach mouse callback to window
- cv2.setMouseCallback(ar_environment.name, on_mouse_event)
-
- # Enable movie video capture
- video_capture = cv2.VideoCapture(args.movie)
-
- video_fps = video_capture.get(cv2.CAP_PROP_FPS)
- image_width = int(video_capture.get(cv2.CAP_PROP_FRAME_WIDTH))
- image_height = int(video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
-
- # Enable exit signal handler
- exit = MiscFeatures.ExitSignalHandler()
-
- # Init image selection
- current_image_index = -1
- _, current_image = video_capture.read()
- next_image_index = int(args.start * video_fps)
- refresh = False
-
- # Init marker selection
- scene_markers = {}
- selected_marker_id = -1
- hovered_marker_id = -1
-
- # Init place edition
- place_edit = {}
-
- while not exit.status():
-
- # Edit fake gaze position from pointer
- gaze_position = GazeFeatures.GazePosition(pointer, precision=2)
-
- # Reset info image
- info_image = numpy.full((850, 1500, 3), 0, dtype=numpy.uint8)
-
- # Select a new image and detect markers once
- if next_image_index != current_image_index or refresh or draw_cover:
-
- video_capture.set(cv2.CAP_PROP_POS_FRAMES, next_image_index)
-
- success, video_image = video_capture.read()
-
- if success:
-
- # Refresh once
- refresh = False
-
- current_image_index = video_capture.get(cv2.CAP_PROP_POS_FRAMES) - 1
- current_image_time = video_capture.get(cv2.CAP_PROP_POS_MSEC)
-
- # Hide zone
- if draw_cover:
-
- # Draw black circle under pointer
- cv2.circle(video_image, pointer, 50, (0, 0, 0), -1)
-
- # Detect markers
- ar_environment.aruco_detector.detect_markers(video_image)
-
- # Filter scene markers
- scene_markers, _ = ar_scene.aruco_scene.filter_markers(ar_environment.aruco_detector.detected_markers)
-
- # Estimate scene marker's pose
- ar_environment.aruco_detector.estimate_markers_pose(scene_markers.keys())
-
- # Write scene detected markers
- cv2.putText(video_image, f'{list(scene_markers.keys())}', (20, image_height-80), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
- # Draw gray panel on top
- cv2.rectangle(video_image, (0, 0), (image_width, 50), (63, 63, 63), -1)
-
- # Draw camera calibration
- if draw_grid:
-
- cv2.putText(video_image, f'Grid at {z_grid} cm', (500, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- ar_environment.aruco_detector.optic_parameters.draw(video_image, image_width/10, image_height/10, z_grid, color=(127, 127, 127))
-
- # Write timing
- cv2.putText(video_image, f'Time: {int(current_image_time)} ms', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
- # Copy image
- current_image = video_image.copy()
-
- # Keep last image
- else:
-
- video_image = current_image.copy()
-
- # Handle scene marker selection on left click
- if len(scene_markers) > 0:
-
- # Update selected and hovered markers id
- selected_marker_id = -1
- hovered_marker_id = -1
- for (marker_id, marker) in scene_markers.items():
-
- marker_aoi = marker.corners.reshape(4, 2).view(AOIFeatures.AreaOfInterest)
-
- # Select by left clicking on marker
- if marker_aoi.contains_point(left_click):
-
- selected_marker_id = marker_id
-
- # Hover by pointing on marker
- if marker_aoi.contains_point(pointer):
-
- hovered_marker_id = marker_id
-
- # Edit marker's color
- color_list = list(itertools.permutations([0, 255, 255]))
-
- for i, m in scene_markers.items():
-
- m.color = color_list[i%len(color_list)]
-
- if i == selected_marker_id or i == hovered_marker_id:
- continue
-
- if hovered_marker_id > 0:
- m.color = (0, 0, 0)
- else:
- m.color = (127, 127, 127)
-
- # Draw center
- cv2.circle(video_image, m.center.astype(int), 5, m.color, -1)
-
- try:
-
- # A marker is selected
- if selected_marker_id >= 0:
-
- try:
-
- # Retreive selected marker
- selected_marker = scene_markers[selected_marker_id]
-
- # Write selected marker id
- cv2.rectangle(info_image, (0, 0), (500, 50), selected_marker.color, -1)
- cv2.putText(info_image, f'Selected marker #{selected_marker.identifier}', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.rectangle(info_image, (0, 50), (500, image_height), (255, 255, 255), -1)
-
- # Write selected marker rotation matrix
- R = ArUcoScene.make_euler_rotation_vector(selected_marker.rotation)
- cv2.putText(info_image, f'Rotation (camera axis)', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[0]:.3f}', (40, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[1]:.3f}', (40, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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_image, f'Translation (camera axis):', (20, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[0]:.3f}', (40, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[1]:.3f}', (40, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[2]:.3f}', (40, 440), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA)
-
- # Retreive selected marker place
- selected_place = ar_scene.aruco_scene.places[selected_marker_id]
-
- # On right click
- if right_button:
-
- pointer_delta_x, pointer_delta_y = right_drag[0] / image_width, right_drag[1] / image_height
-
- place_edit[selected_marker_id] = {'rotation': (0, 0, 0), 'translation': (0, 0, 0)}
-
- if edit_trans:
-
- # Edit place rotation
- if edit_z:
- place_edit[selected_marker_id]['rotation'] = (0, 0, -pointer_delta_y)
- else:
- place_edit[selected_marker_id]['rotation'] = (pointer_delta_y, pointer_delta_x, 0)
-
- else:
-
- # Edit place translation
- if edit_z:
- place_edit[selected_marker_id]['translation'] = (0, 0, pointer_delta_y)
- else:
- place_edit[selected_marker_id]['translation'] = (-pointer_delta_x, pointer_delta_y, 0)
-
- # Edit transformations
- R = ArUcoScene.make_rotation_matrix(*place_edit[selected_marker_id]['rotation']).T
- T = numpy.array(place_edit[selected_marker_id]['translation'])
-
- # Apply transformations
- edited_place = ArUcoScene.Place(selected_place.translation + T, selected_place.rotation.dot(R), selected_marker)
-
- else:
-
- edited_place = selected_place
-
- # 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 = scene_markers[hovered_marker_id]
-
- # Write hovered marker id
- cv2.rectangle(info_image, (500, 0), (1000, 50), hovered_marker.color, -1)
- cv2.putText(info_image, f'Hovered marker #{hovered_marker.identifier}', (520, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.rectangle(info_image, (500, 50), (1000, image_height), (255, 255, 255), -1)
-
- # Write hovered marker rotation matrix
- R = ArUcoScene.make_euler_rotation_vector(hovered_marker.rotation)
- cv2.putText(info_image, f'Rotation (camera axis)', (520, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[0]:.3f}', (540, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[1]:.3f}', (540, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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_image, f'Translation (camera axis):', (520, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[0]:.3f}', (540, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[1]:.3f}', (540, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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 hovered place rotation matrix
- R = ArUcoScene.make_euler_rotation_vector(hovered_place.rotation)
- cv2.putText(info_image, f'Rotation (scene axis):', (520, 500), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[0]:.3f}', (540, 540), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[1]:.3f}', (540, 580), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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_image, f'Translation (scene axis):', (520, 700), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[0]:.3f}', (540, 740), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[1]:.3f}', (540, 780), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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)
-
- # 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_image, (1000, 0), (1500, 50), (63, 63, 63), -1)
- cv2.putText(info_image, f'#{selected_marker.identifier} -> #{hovered_marker.identifier}', (1020, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- cv2.rectangle(info_image, (1000, 50), (1500, image_height), (190, 190, 190), -1)
-
- # Write selected/hovered markers rotation matrix
- R = markers_rotation_vector
- cv2.putText(info_image, f'Rotation (camera axis)', (1020, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[0]:.3f}', (1040, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[1]:.3f}', (1040, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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_image, f'Translation (camera axis):', (1020, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[0]:.3f}', (1040, 360), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[1]:.3f}', (1040, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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_image, f'Rotation (scene axis):', (1020, 500), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[0]:.3f}', (1040, 540), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[1]:.3f}', (1040, 580), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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_image, f'Translation (scene axis):', (1020, 700), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[0]:.3f}', (1040, 740), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[1]:.3f}', (1040, 780), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[2]:.3f}', (1040, 820), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 1, cv2.LINE_AA)
-
- if snap:
-
- # Snap once
- snap = False
-
- 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 = ArUcoScene.make_euler_rotation_vector(edited_place.rotation)
- cv2.putText(info_image, f'Rotation (scene axis):', (20, 500), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[0]:.3f}', (40, 540), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{R[1]:.3f}', (40, 580), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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_image, f'Translation (scene axis):', (20, 700), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[0]:.3f}', (40, 740), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- cv2.putText(info_image, f'{T[1]:.3f}', (40, 780), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv2.LINE_AA)
- cv2.putText(info_image, 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
-
- # Refresh places consistency
- ar_scene.aruco_scene.init_places_consistency()
-
- # Estimate scene pose from each marker
- cv2.putText(video_image, f'Single marker scene pose estimation', (20, image_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
- for i, m in scene_markers.items():
-
- tvec, rmat = ar_scene.aruco_scene.estimate_pose_from_single_marker(m)
-
- # Project AOI scene into image 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_image, gaze_position, 1, base_color=m.color, matching_color=(255, 255, 255))
-
- else:
-
- # Draw AOI scene
- aoi_scene_projection.draw(video_image, color=m.color)
-
- # Draw expected marker places
- ar_scene.draw_places(video_image)
-
- # Catch missing selected marker
- except KeyError:
-
- cv2.putText(video_image, f'Marker {selected_marker_id} not found', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
-
- # No marker selected
- else:
-
- cv2.putText(info_image, f'Left click on marker to select it', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-
- if len(scene_markers) > 1:
-
- # Check markers consistency
- consistent_markers, unconsistent_markers, unconsistencies = ar_scene.aruco_scene.check_markers_consistency(scene_markers, ar_scene.angle_tolerance, ar_scene.distance_tolerance)
-
- # No marker hovered
- if hovered_marker_id < 0:
-
- # Set unconsistent marker color to red
- for i, m in scene_markers.items():
- if i in list(unconsistent_markers.keys()) and i != hovered_marker_id:
- m.color = (0, 0, 255)
-
- # Write unconsistencies
- line = 0
- for i, (label, value) in enumerate(unconsistencies['rotation'].items()):
-
- current_rotation = value['current']
- expected_rotation = value['expected']
-
- cv2.putText(info_image, f'Unconsistent rotation {label}: [{current_rotation[0]:.3f} {current_rotation[1]:.3f} {current_rotation[2]:.3f}]', (20, 120+line*40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- line += 1
-
- cv2.putText(info_image, f'Expected rotation {label}: [{expected_rotation[0]:.3f} {expected_rotation[1]:.3f} {expected_rotation[2]:.3f}]', (20, 120+line*40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- line += 1
-
- for i, (label, value) in enumerate(unconsistencies['translation'].items()):
-
- current_translation = value['current']
- expected_translation = value['expected']
-
- cv2.putText(info_image, f'Unconsistent translation {label}: {current_translation:.3f}', (20, 120+ line*40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
- line += 1
-
- cv2.putText(info_image, f'Expected translation {label}: {expected_translation:.3f}', (20, 120+ line*40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- line += 1
-
- # Force pose mode to single marker scene pose estimation
- else:
-
- pose_mode = 0
-
- # Single marker scene pose estimation
- if pose_mode == 0:
-
- cv2.putText(video_image, f'Single marker scene pose estimation', (20, image_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
- for i, m in scene_markers.items():
-
- tvec, rmat = ar_scene.aruco_scene.estimate_pose_from_single_marker(m)
-
- # Project AOI scene into image according estimated pose
- aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV)
-
- # Draw AOI scene
- aoi_scene_projection.draw(video_image, color=m.color)
-
- # Consistent markers scene pose estimation
- if pose_mode == 1:
-
- cv2.putText(video_image, f'Consistent markers scene pose estimation', (20, image_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 image according estimated pose
- aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV)
-
- # Draw AOI scene
- aoi_scene_projection.draw(video_image, color=(255, 255, 255))
-
- # ArUco marker axis scene pose estimation
- elif pose_mode == 2:
-
- # Write pose estimation strategy
- cv2.putText(video_image, f'ArUco marker axis scene pose estimation', (20, image_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
- for axis_name, axis_markers in ar_scene.aruco_axis.items():
-
- try:
-
- origin_marker = scene_markers[axis_markers['origin_marker']]
- horizontal_axis_marker = scene_markers[axis_markers['horizontal_axis_marker']]
- vertical_axis_marker = scene_markers[axis_markers['vertical_axis_marker']]
-
- tvec, rmat = ar_scene.aruco_scene.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker)
-
- # Project AOI scene into image according estimated pose
- aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV)
-
- # Draw AOI scene
- aoi_scene_projection.draw(video_image, color=(255, 255, 255))
-
- break
-
- except:
- pass
-
- # ArUco AOI scene building
- elif pose_mode == 3:
-
- # Write pose estimation strategy
- cv2.putText(video_image, f'ArUco AOI scene building', (20, image_height - 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
- try :
-
- # Try to build AOI scene from detected ArUco marker corners
- aoi_scene_projection = ar_scene.build_aruco_aoi_scene(scene_markers)
-
- # Draw AOI scene
- aoi_scene_projection.draw(video_image, color=(255, 255, 255))
-
- except:
- pass
-
- # Draw expected marker places
- #ar_scene.draw_places(video_image)
-
- # Catch exceptions raised by estimate_pose and project methods
- except (ArFeatures.PoseEstimationFailed, ArFeatures.SceneProjectionFailed) as e:
-
- cv2.rectangle(video_image, (0, 90), (700, 130), (127, 127, 127), -1)
- cv2.putText(video_image, f'Error: {e}', (20, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-
- # Draw image
- cv2.imshow(ar_environment.name, video_image)
-
- # Draw detected markers
- ar_environment.aruco_detector.draw_detected_markers(video_image)
-
- # Draw pointer
- gaze_position.draw(video_image)
-
- # Write documentation
- cv2.putText(video_image, f'Press \'h\' for help', (950, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-
- if draw_help:
-
- cv2.rectangle(video_image, (0, 50), (700, 300), (127, 127, 127), -1)
- cv2.putText(video_image, f'> Left click on marker: select marker', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(video_image, f'> Left click on image: unselect marker', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(video_image, f'> T: translate, R: rotate, Z: select axis', (20, 160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(video_image, f'> Right click and drag: edit axis', (20, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(video_image, f'> Ctrl + S: save environment', (20, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(video_image, f'> Backspace: reload environment', (20, 280), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
-
- # Write selected marker id
- elif selected_marker_id >= 0:
-
- cv2.rectangle(video_image, (0, 50), (700, 90), (127, 127, 127), -1)
-
- # Select color
- if edit_z:
- str_axis = 'Z'
- color_axis = (255, 0, 0)
- else:
- str_axis = 'XY'
- color_axis = (0, 255, 255)
-
- if edit_trans:
- cv2.putText(video_image, f'Rotate marker {selected_marker_id} around axis {str_axis}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, color_axis, 1, cv2.LINE_AA)
- else:
- cv2.putText(video_image, f'Translate marker {selected_marker_id} along axis {str_axis}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, color_axis, 1, cv2.LINE_AA)
-
- key_pressed = cv2.waitKey(10)
-
- #if key_pressed != -1:
- # print(key_pressed)
-
- # Select previous image with left arrow
- if key_pressed == 2:
- next_image_index -= 1
-
- # Select next image with right arrow
- if key_pressed == 3:
- next_image_index += 1
-
- # Clip image index
- if next_image_index < 0:
- next_image_index = 0
-
- # Edit rotation with r key
- if key_pressed == 114:
- edit_trans = True
-
- # Edit translation with t key
- if key_pressed == 116:
- edit_trans = False
-
- # Switch Z axis edition
- 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
-
- # Switch grid mode with g key
- if key_pressed == 103:
- draw_grid = not draw_grid
- refresh = True
-
- # Raise z grid with down arrow
- if key_pressed == 0:
- z_grid += 10.
- refresh = True
-
- # Unraise z grid with up arrow
- if key_pressed == 1:
- z_grid -= 10.
- refresh = True
-
- # Switch draw_cover mode with c key
- if key_pressed == 99:
- draw_cover = not draw_cover
-
- # Switch pose estimation mode with m key
- if key_pressed == 109:
- pose_mode += 1
- if pose_mode > 3:
- pose_mode = 0
-
- # Save selected marker edition using 'Ctrl + s'
- if key_pressed == 19:
- ar_environment.to_json(args.output)
- print(f'Environment saved into {args.output}')
-
- # Close window using 'Esc' key
- if key_pressed == 27:
- break
-
- # Reload environment on 'Backspace' key
- if key_pressed == 127:
- ar_environment = ArFeatures.ArEnvironment.from_json(args.environment)
- print(f'Environment reloaded from {args.environment}')
- refresh = True
-
- # Display video
- cv2.imshow(ar_environment.name, video_image)
-
- # Display info
- cv2.imshow('Info', info_image)
-
- # Close movie capture
- video_capture.release()
-
- # Stop image display
- cv2.destroyAllWindows()
-
-if __name__ == '__main__':
-
- main() \ No newline at end of file