aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/utils/aruco_markers_group_export.py110
1 files changed, 82 insertions, 28 deletions
diff --git a/src/argaze/utils/aruco_markers_group_export.py b/src/argaze/utils/aruco_markers_group_export.py
index 92646ca..5012d01 100644
--- a/src/argaze/utils/aruco_markers_group_export.py
+++ b/src/argaze/utils/aruco_markers_group_export.py
@@ -11,7 +11,7 @@ import argparse
import time
import itertools
-from argaze.ArUcoMarkers import ArUcoCamera, ArUcoMarkersGroup
+from argaze.ArUcoMarkers import ArUcoDetector, ArUcoOpticCalibrator, ArUcoMarkersGroup
from argaze.utils import UtilsFeatures
import cv2
@@ -19,16 +19,22 @@ import numpy
def main():
"""
- Load a MOVIE and an ArUcoCamera CONFIGURATION to detect ArUco markers inside a selected movie frame then, export detected ArUco markers group as .obj file into an OUTPUT folder.
+ Detect DICTIONARY and SIZE ArUco markers inside a MOVIE frame then, export detected ArUco markers group as .obj file into an OUTPUT folder.
"""
# Manage arguments
parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
parser.add_argument('movie', metavar='MOVIE', type=str, default=None, help='movie path')
- parser.add_argument('configuration', metavar='CONFIGURATION', type=str, default=None, help='ArUco camera configuration')
+ parser.add_argument('dictionary', metavar='DICTIONARY', type=str, default=None, help='expected ArUco markers dictionary')
+ parser.add_argument('size', metavar='SIZE', type=float, default=None, help='expected ArUco markers size (in cm)')
- parser.add_argument('-s','--start', metavar='START', type=float, default=0., help='start time in second')
+ parser.add_argument('-p', '--parameters', metavar='PARAMETERS', type=str, default=None, help='ArUco detector parameters file')
+ parser.add_argument('-op', '--optic_parameters', metavar='OPTIC_PARAMETERS', type=str, default=None, help='ArUco detector optic parameters file')
+
+ parser.add_argument('-s', '--start', metavar='START', type=float, default=0., help='start time in second')
parser.add_argument('-o', '--output', metavar='OUTPUT', type=str, default='.', help='export folder path')
+ parser.add_argument('-v', '--verbose', action='store_true', default=False, help='enable verbose mode to print information in console')
+
args = parser.parse_args()
# Load movie
@@ -38,14 +44,39 @@ def main():
image_width = int(video_capture.get(cv2.CAP_PROP_FRAME_WIDTH))
image_height = int(video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
- # Load ArUco camera
- aruco_camera = ArUcoCamera.ArUcoCamera.from_json(args.configuration)
+ # Edit ArUco detector configuration
+ configuration = {
+ "dictionary": args.dictionary
+ }
+
+ if args.parameters:
+
+ configuration["parameters"] = args.parameters
+
+ if args.optic_parameters:
+
+ configuration["optic_parameters"] = args.optic_parameters
+
+ # Load ArUco detector configuration
+ aruco_detector = ArUcoDetector.ArUcoDetector.from_dict(configuration, '.')
+
+ if args.verbose:
+
+ print(aruco_detector)
# Create empty ArUco scene
aruco_markers_group = None
+ # Edit draw parameters
+ draw_parameters = {
+ "color": [255, 255, 255],
+ "draw_axes": {
+ "thickness": 4
+ }
+ }
+
# Create a window
- cv2.namedWindow(aruco_camera.name, cv2.WINDOW_AUTOSIZE)
+ cv2.namedWindow("Export detected ArUco markers", cv2.WINDOW_AUTOSIZE)
# Enable exit signal handler
exit = UtilsFeatures.ExitSignalHandler()
@@ -65,6 +96,14 @@ def main():
success, video_image = video_capture.read()
+ video_height, video_width, _ = video_image.shape
+
+ # Create default optic parameters adapted to frame size
+ if aruco_detector.optic_parameters is None:
+
+ # Note: The choice of 1000 for default focal length should be discussed...
+ aruco_detector.optic_parameters = ArUcoOpticCalibrator.OpticParameters(rms=-1, dimensions=(video_width, video_height), K=ArUcoOpticCalibrator.K0(focal_length=(1000., 1000.), width=video_width, height=video_height))
+
if success:
# Refresh once
@@ -73,34 +112,49 @@ def main():
current_image_index = video_capture.get(cv2.CAP_PROP_POS_FRAMES) - 1
current_image_time = video_capture.get(cv2.CAP_PROP_POS_MSEC)
- # Detect markers
- detection_time, projection_time, exceptions = aruco_camera.watch(current_image_time, video_image)
+ try:
+
+ # Detect and project AR features
+ aruco_detector.detect_markers(current_image_time, video_image)
+
+ # Estimate all detected markers pose
+ aruco_detector.estimate_markers_pose(args.size)
- # Estimate each markers pose
- aruco_camera.aruco_detector.estimate_markers_pose(aruco_camera.aruco_detector.detected_markers)
+ # Build aruco scene from detected markers
+ aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup(aruco_detector.dictionary, aruco_detector.detected_markers)
- # Build aruco scene from detected markers
- aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup(aruco_camera.aruco_detector.marker_size, aruco_camera.aruco_detector.dictionary, aruco_camera.aruco_detector.detected_markers)
+ # Detection suceeded
+ exception = None
- # Get camera image
- camera_image = aruco_camera.image()
+ # Write errors
+ except Exception as e:
+
+ aruco_markers_group = None
+
+ exception = e
+
+ # Draw detected markers
+ aruco_detector.draw_detected_markers(video_image, draw_parameters)
# Write detected markers
- cv2.putText(camera_image, f'Detecting markers {list(aruco_camera.aruco_detector.detected_markers.keys())}', (20, aruco_camera.size[1]-40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+ cv2.putText(video_image, f'Detecting markers {list(aruco_detector.detected_markers.keys())}', (20, video_height-40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
# Write timing
- cv2.putText(camera_image, f'Frame at {int(current_image_time)}ms', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(camera_image, f'Detection {int(detection_time)}ms', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(camera_image, f'Projection {int(projection_time)}ms', (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
-
- # Write documentation
- cv2.putText(camera_image, f'<- previous image', (aruco_camera.size[0]-500, aruco_camera.size[1]-160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(camera_image, f'-> next image', (aruco_camera.size[0]-500, aruco_camera.size[1]-120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(camera_image, f'r: reload config', (aruco_camera.size[0]-500, aruco_camera.size[1]-80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
- cv2.putText(camera_image, f'Ctrl+s: export ArUco markers', (aruco_camera.size[0]-500, aruco_camera.size[1]-40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
+ cv2.putText(video_image, f'Frame at {int(current_image_time)}ms', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+
+ # Write exception
+ if exception is not None:
+
+ cv2.putText(video_image, f'error: {exception}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
+ # Write documentation
+ cv2.putText(video_image, f'<- previous image', (video_width-500, video_height-160), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
+ cv2.putText(video_image, f'-> next image', (video_width-500, video_height-120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
+ cv2.putText(video_image, f'r: reload config', (video_width-500, video_height-80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
+ cv2.putText(video_image, f'Ctrl+s: export ArUco markers', (video_width-500, video_height-40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
+
# Copy image
- current_image = camera_image.copy()
+ current_image = video_image.copy()
# Keep last image
else:
@@ -127,7 +181,7 @@ def main():
# r: reload configuration
if key_pressed == 114:
- aruco_camera = ArUcoCamera.ArUcoCamera.from_json(args.configuration)
+ aruco_detector = ArUcoDetector.ArUcoDetector.from_dict(configuration)
refresh = True
print('Configuration reloaded')
@@ -148,7 +202,7 @@ def main():
break
# Display video
- cv2.imshow(aruco_camera.name, video_image)
+ cv2.imshow(aruco_detector.name, video_image)
# Close movie capture
video_capture.release()