aruco-pose 0.2.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- aruco_pose/__init__.py +26 -0
- aruco_pose/__main__.py +6 -0
- aruco_pose/cli.py +403 -0
- aruco_pose/core/__init__.py +7 -0
- aruco_pose/core/detector.py +191 -0
- aruco_pose/kit_cli.py +150 -0
- aruco_pose/models/__init__.py +18 -0
- aruco_pose/models/aruco_pose_estimator.py +109 -0
- aruco_pose/models/aruco_pose_pipeline.py +90 -0
- aruco_pose/models/dodecahedron_model.py +196 -0
- aruco_pose/pose.py +702 -0
- aruco_pose/starter.py +393 -0
- aruco_pose/starter_sample.py +78 -0
- aruco_pose/tracking/__init__.py +7 -0
- aruco_pose/tracking/dense_refinement.py +165 -0
- aruco_pose/tracking/optical_flow.py +15 -0
- aruco_pose/tracking/pose_refiner.py +23 -0
- aruco_pose/tracking/predictive_filter.py +30 -0
- aruco_pose/utils/__init__.py +19 -0
- aruco_pose/utils/config.py +199 -0
- aruco_pose/utils/dodecahedron_overlay.py +19 -0
- aruco_pose/utils/io.py +296 -0
- aruco_pose/utils/legacy_utils.py +445 -0
- aruco_pose/utils/logger.py +80 -0
- aruco_pose/utils/marker_generator.py +192 -0
- aruco_pose/utils/performance.py +228 -0
- aruco_pose/utils/visualization.py +276 -0
- aruco_pose-0.2.0.dist-info/LICENSE +201 -0
- aruco_pose-0.2.0.dist-info/METADATA +240 -0
- aruco_pose-0.2.0.dist-info/RECORD +33 -0
- aruco_pose-0.2.0.dist-info/WHEEL +5 -0
- aruco_pose-0.2.0.dist-info/entry_points.txt +3 -0
- aruco_pose-0.2.0.dist-info/top_level.txt +1 -0
aruco_pose/__init__.py
ADDED
|
@@ -0,0 +1,26 @@
|
|
|
1
|
+
"""
|
|
2
|
+
aruco-pose: rigid marker pose estimation
|
|
3
|
+
A robust toolkit for ArUco marker detection, pose estimation, and custom 3D model tracking.
|
|
4
|
+
"""
|
|
5
|
+
|
|
6
|
+
__version__ = "0.2.0"
|
|
7
|
+
__author__ = "Jonathan Shulgach"
|
|
8
|
+
|
|
9
|
+
from .core.detector import UnifiedArucoDetector
|
|
10
|
+
from .pose import Tracker, TrackerConfig, Pose, PoseResult, PoseCandidate, MarkerObservation
|
|
11
|
+
from .utils.visualization import ArucoVisualizer
|
|
12
|
+
from .utils.config import ProjectConfig, CameraConfig, CalibrationConfig, ArucoConfig
|
|
13
|
+
from .utils.performance import PerformanceMonitor
|
|
14
|
+
from .utils.logger import setup_logger
|
|
15
|
+
|
|
16
|
+
__all__ = [
|
|
17
|
+
'Tracker', 'TrackerConfig', 'Pose', 'PoseResult', 'PoseCandidate', 'MarkerObservation',
|
|
18
|
+
'UnifiedArucoDetector',
|
|
19
|
+
'ArucoVisualizer',
|
|
20
|
+
'ProjectConfig',
|
|
21
|
+
'CameraConfig',
|
|
22
|
+
'CalibrationConfig',
|
|
23
|
+
'ArucoConfig',
|
|
24
|
+
'PerformanceMonitor',
|
|
25
|
+
'setup_logger',
|
|
26
|
+
]
|
aruco_pose/__main__.py
ADDED
aruco_pose/cli.py
ADDED
|
@@ -0,0 +1,403 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Command-line interface for ArUco Pose Estimation toolkit.
|
|
3
|
+
"""
|
|
4
|
+
|
|
5
|
+
import click
|
|
6
|
+
import cv2
|
|
7
|
+
import numpy as np
|
|
8
|
+
import sys
|
|
9
|
+
from pathlib import Path
|
|
10
|
+
import time
|
|
11
|
+
|
|
12
|
+
|
|
13
|
+
from . import __version__
|
|
14
|
+
from aruco_pose.core.detector import UnifiedArucoDetector
|
|
15
|
+
from aruco_pose.utils.config import ProjectConfig
|
|
16
|
+
from aruco_pose.utils.visualization import ArucoVisualizer
|
|
17
|
+
from aruco_pose.utils.performance import PerformanceMonitor
|
|
18
|
+
from aruco_pose.utils.logger import setup_logger
|
|
19
|
+
from aruco_pose.utils.io import VideoHandler, PoseRecorder
|
|
20
|
+
|
|
21
|
+
|
|
22
|
+
@click.group(name="aruco-pose")
|
|
23
|
+
@click.version_option(version=__version__)
|
|
24
|
+
def cli():
|
|
25
|
+
"""aruco-pose: marker detection, calibration and rigid-object pose tracking."""
|
|
26
|
+
pass
|
|
27
|
+
|
|
28
|
+
|
|
29
|
+
@cli.command()
|
|
30
|
+
@click.option("--config", default="config.yaml", help="Path to config file")
|
|
31
|
+
@click.option(
|
|
32
|
+
"--visualize/--no-visualize", default=True, help="Show visualization during capture"
|
|
33
|
+
)
|
|
34
|
+
@click.option("--have-frames", is_flag=True, help="Skip capturing new frames")
|
|
35
|
+
def calibrate(config, visualize, have_frames):
|
|
36
|
+
"""Legacy calibration command; prefer `kit calibrate`."""
|
|
37
|
+
logger = setup_logger("calibrate", level=10 if visualize else 20)
|
|
38
|
+
|
|
39
|
+
try:
|
|
40
|
+
# Load configuration
|
|
41
|
+
if Path(config).exists():
|
|
42
|
+
cfg = ProjectConfig.from_legacy_yaml(config)
|
|
43
|
+
else:
|
|
44
|
+
logger.warning(f"Config file not found: {config}, using defaults")
|
|
45
|
+
cfg = ProjectConfig()
|
|
46
|
+
|
|
47
|
+
cfg.validate()
|
|
48
|
+
|
|
49
|
+
logger.info("Starting camera calibration...")
|
|
50
|
+
logger.info(
|
|
51
|
+
f"Checkerboard: {cfg.calibration.checkerboard_rows}x{cfg.calibration.checkerboard_cols}"
|
|
52
|
+
)
|
|
53
|
+
logger.info(f"Square size: {cfg.calibration.square_size} m")
|
|
54
|
+
|
|
55
|
+
# Import calibration utilities
|
|
56
|
+
from aruco_pose.utils.legacy_utils import (
|
|
57
|
+
save_frames_single_camera,
|
|
58
|
+
calibrate_single_camera,
|
|
59
|
+
)
|
|
60
|
+
|
|
61
|
+
calib = {
|
|
62
|
+
"camera0": cfg.camera.device_id,
|
|
63
|
+
"frame_width": cfg.camera.width,
|
|
64
|
+
"frame_height": cfg.camera.height,
|
|
65
|
+
"img_dir": cfg.calibration.img_dir,
|
|
66
|
+
"n_frames": cfg.calibration.n_frames,
|
|
67
|
+
"cooldown": cfg.calibration.cooldown,
|
|
68
|
+
"view_resize": cfg.calibration.view_resize,
|
|
69
|
+
"checkerboard_row_vertices": cfg.calibration.checkerboard_rows,
|
|
70
|
+
"checkerboard_column_vertices": cfg.calibration.checkerboard_cols,
|
|
71
|
+
"checkerboard_box_size_scale": cfg.calibration.square_size,
|
|
72
|
+
}
|
|
73
|
+
|
|
74
|
+
if not have_frames:
|
|
75
|
+
logger.info("Capturing calibration frames...")
|
|
76
|
+
save_frames_single_camera(calib, visualize)
|
|
77
|
+
|
|
78
|
+
logger.info("Computing calibration matrices...")
|
|
79
|
+
_, mtx, dist = calibrate_single_camera(calib, visualize)
|
|
80
|
+
|
|
81
|
+
# Save results
|
|
82
|
+
np.save("camera_intrinsics.npy", mtx)
|
|
83
|
+
np.save("camera_distortion.npy", dist)
|
|
84
|
+
|
|
85
|
+
logger.info("✓ Calibration complete!")
|
|
86
|
+
logger.info(f" Saved: camera_intrinsics.npy, camera_distortion.npy")
|
|
87
|
+
|
|
88
|
+
except Exception as e:
|
|
89
|
+
logger.error(f"Calibration failed: {e}")
|
|
90
|
+
sys.exit(1)
|
|
91
|
+
|
|
92
|
+
|
|
93
|
+
@cli.command()
|
|
94
|
+
@click.option("--config", default="config.yaml", help="Path to config file")
|
|
95
|
+
@click.option("--source", type=str, default="0", help="Camera ID or video path")
|
|
96
|
+
@click.option("--calibration", help="Path to calibration matrix (.npy)")
|
|
97
|
+
@click.option("--distortion", help="Path to distortion coefficients (.npy)")
|
|
98
|
+
@click.option("--output", help="Output video file (optional)")
|
|
99
|
+
@click.option("--record", help="Record pose data to JSON file")
|
|
100
|
+
@click.option(
|
|
101
|
+
"--show-axes/--no-show-axes", default=True, help="Draw 3D axes on markers"
|
|
102
|
+
)
|
|
103
|
+
@click.option("--show-ids/--no-show-ids", default=True, help="Show marker IDs")
|
|
104
|
+
@click.option(
|
|
105
|
+
"--show-stats/--no-show-stats", default=True, help="Show performance stats"
|
|
106
|
+
)
|
|
107
|
+
@click.option("--marker-size", type=float, help="Physical marker size in meters")
|
|
108
|
+
def track(
|
|
109
|
+
config,
|
|
110
|
+
source,
|
|
111
|
+
calibration,
|
|
112
|
+
distortion,
|
|
113
|
+
output,
|
|
114
|
+
record,
|
|
115
|
+
show_axes,
|
|
116
|
+
show_ids,
|
|
117
|
+
show_stats,
|
|
118
|
+
marker_size,
|
|
119
|
+
):
|
|
120
|
+
"""
|
|
121
|
+
Legacy tracker; prefer `kit track` for shared poses, quality checks and CSV.
|
|
122
|
+
|
|
123
|
+
Press 'q' to quit, 's' to save current frame, 'r' to reset statistics.
|
|
124
|
+
"""
|
|
125
|
+
logger = setup_logger("track")
|
|
126
|
+
|
|
127
|
+
try:
|
|
128
|
+
# Load configuration
|
|
129
|
+
if Path(config).exists():
|
|
130
|
+
cfg = ProjectConfig.from_legacy_yaml(config)
|
|
131
|
+
else:
|
|
132
|
+
logger.warning(f"Config file not found: {config}, using defaults")
|
|
133
|
+
cfg = ProjectConfig()
|
|
134
|
+
|
|
135
|
+
# Override with command line arguments
|
|
136
|
+
if marker_size:
|
|
137
|
+
cfg.aruco.marker_size = marker_size
|
|
138
|
+
|
|
139
|
+
# Load calibration
|
|
140
|
+
K = None
|
|
141
|
+
D = None
|
|
142
|
+
if calibration or Path("camera_intrinsics.npy").exists():
|
|
143
|
+
calib_path = calibration or "camera_intrinsics.npy"
|
|
144
|
+
dist_path = distortion or "camera_distortion.npy"
|
|
145
|
+
|
|
146
|
+
if Path(calib_path).exists() and Path(dist_path).exists():
|
|
147
|
+
K = np.load(calib_path)
|
|
148
|
+
D = np.load(dist_path)
|
|
149
|
+
logger.info(f"Loaded calibration from {calib_path}")
|
|
150
|
+
else:
|
|
151
|
+
logger.warning(
|
|
152
|
+
"Calibration files not found, running without pose estimation"
|
|
153
|
+
)
|
|
154
|
+
|
|
155
|
+
# Initialize components
|
|
156
|
+
detector = UnifiedArucoDetector(cfg.aruco.dict_type)
|
|
157
|
+
visualizer = ArucoVisualizer(K, D)
|
|
158
|
+
monitor = PerformanceMonitor()
|
|
159
|
+
|
|
160
|
+
# Initialize video handler
|
|
161
|
+
try:
|
|
162
|
+
source_val = int(source)
|
|
163
|
+
except ValueError:
|
|
164
|
+
source_val = source
|
|
165
|
+
|
|
166
|
+
video = VideoHandler(source_val, cfg.camera.width, cfg.camera.height)
|
|
167
|
+
logger.info(f"Opened video source: {source}")
|
|
168
|
+
|
|
169
|
+
# Initialize recorder if requested
|
|
170
|
+
recorder = None
|
|
171
|
+
if record:
|
|
172
|
+
recorder = PoseRecorder(record, save_interval=100)
|
|
173
|
+
|
|
174
|
+
# Initialize video writer if output requested
|
|
175
|
+
writer = None
|
|
176
|
+
if output:
|
|
177
|
+
props = video.get_properties()
|
|
178
|
+
fourcc = cv2.VideoWriter_fourcc(*"mp4v")
|
|
179
|
+
writer = cv2.VideoWriter(
|
|
180
|
+
output, fourcc, 30.0, (props["width"], props["height"])
|
|
181
|
+
)
|
|
182
|
+
|
|
183
|
+
logger.info("Starting tracking... (Press 'q' to quit)")
|
|
184
|
+
frame_count = 0
|
|
185
|
+
|
|
186
|
+
while True:
|
|
187
|
+
monitor.start_frame()
|
|
188
|
+
|
|
189
|
+
ret, frame = video.read()
|
|
190
|
+
if not ret:
|
|
191
|
+
logger.warning("Failed to read frame")
|
|
192
|
+
break
|
|
193
|
+
|
|
194
|
+
# Detect markers
|
|
195
|
+
monitor.start_detection()
|
|
196
|
+
corners, ids, rejected = detector.detect(frame)
|
|
197
|
+
n_markers = len(ids) if ids is not None else 0
|
|
198
|
+
monitor.end_detection(n_markers)
|
|
199
|
+
|
|
200
|
+
# Draw markers
|
|
201
|
+
frame = visualizer.draw_markers(
|
|
202
|
+
frame,
|
|
203
|
+
corners,
|
|
204
|
+
ids,
|
|
205
|
+
draw_ids=show_ids,
|
|
206
|
+
draw_axes=show_axes and K is not None,
|
|
207
|
+
marker_size=cfg.aruco.marker_size,
|
|
208
|
+
)
|
|
209
|
+
|
|
210
|
+
# Estimate pose if calibration available
|
|
211
|
+
rvec, tvec = None, None
|
|
212
|
+
if ids is not None and len(ids) > 0 and K is not None:
|
|
213
|
+
monitor.start_pose_estimation()
|
|
214
|
+
# Use first marker for simple pose estimation
|
|
215
|
+
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
|
|
216
|
+
corners[0], cfg.aruco.marker_size, K, D
|
|
217
|
+
)
|
|
218
|
+
monitor.end_pose_estimation()
|
|
219
|
+
|
|
220
|
+
# Draw pose info
|
|
221
|
+
frame = visualizer.draw_pose_info(frame, rvec, tvec)
|
|
222
|
+
|
|
223
|
+
# Record data if requested
|
|
224
|
+
if recorder and rvec is not None:
|
|
225
|
+
recorder.record_frame(time.time(), rvec, tvec, ids)
|
|
226
|
+
|
|
227
|
+
# Show performance stats
|
|
228
|
+
if show_stats:
|
|
229
|
+
stats = monitor.get_stats()
|
|
230
|
+
display_stats = {
|
|
231
|
+
"FPS": stats["fps"],
|
|
232
|
+
"Markers": n_markers,
|
|
233
|
+
"Frames": frame_count,
|
|
234
|
+
}
|
|
235
|
+
frame = visualizer.draw_performance_stats(
|
|
236
|
+
frame, display_stats, position=(frame.shape[1] - 200, 30)
|
|
237
|
+
)
|
|
238
|
+
|
|
239
|
+
# Save frame to video if requested
|
|
240
|
+
if writer:
|
|
241
|
+
writer.write(frame)
|
|
242
|
+
|
|
243
|
+
# Display
|
|
244
|
+
cv2.imshow("ArUco Tracker", frame)
|
|
245
|
+
|
|
246
|
+
monitor.end_frame()
|
|
247
|
+
frame_count += 1
|
|
248
|
+
|
|
249
|
+
# Handle keyboard input
|
|
250
|
+
key = cv2.waitKey(1) & 0xFF
|
|
251
|
+
if key == ord("q"):
|
|
252
|
+
break
|
|
253
|
+
elif key == ord("s"):
|
|
254
|
+
save_path = f"frame_{frame_count:06d}.png"
|
|
255
|
+
cv2.imwrite(save_path, frame)
|
|
256
|
+
logger.info(f"Saved frame: {save_path}")
|
|
257
|
+
elif key == ord("r"):
|
|
258
|
+
monitor.reset()
|
|
259
|
+
logger.info("Reset statistics")
|
|
260
|
+
|
|
261
|
+
# Cleanup
|
|
262
|
+
video.release()
|
|
263
|
+
cv2.destroyAllWindows()
|
|
264
|
+
|
|
265
|
+
if writer:
|
|
266
|
+
writer.release()
|
|
267
|
+
logger.info(f"Saved video: {output}")
|
|
268
|
+
|
|
269
|
+
if recorder:
|
|
270
|
+
recorder.save()
|
|
271
|
+
stats = recorder.get_statistics()
|
|
272
|
+
logger.info(f"Recorded {stats['total_frames']} frames")
|
|
273
|
+
|
|
274
|
+
# Print final statistics
|
|
275
|
+
logger.info("\n" + monitor.get_summary())
|
|
276
|
+
logger.info("✓ Tracking complete!")
|
|
277
|
+
|
|
278
|
+
except Exception as e:
|
|
279
|
+
logger.error(f"Tracking failed: {e}", exc_info=True)
|
|
280
|
+
sys.exit(1)
|
|
281
|
+
|
|
282
|
+
|
|
283
|
+
@cli.command()
|
|
284
|
+
@click.argument("marker_ids", nargs=-1, type=int, required=True)
|
|
285
|
+
@click.option(
|
|
286
|
+
"--type",
|
|
287
|
+
"--dict-type",
|
|
288
|
+
"dict_type",
|
|
289
|
+
default="DICT_5X5_100",
|
|
290
|
+
help="ArUco dictionary type",
|
|
291
|
+
)
|
|
292
|
+
@click.option("--size", type=int, default=200, help="Marker size in pixels")
|
|
293
|
+
@click.option("--output", "-o", default="markers", help="Output directory")
|
|
294
|
+
@click.option("--sheet", is_flag=True, help="Generate a single printable sheet")
|
|
295
|
+
@click.option("--markers-per-row", type=int, default=4, help="Markers per row in sheet")
|
|
296
|
+
def generate(marker_ids, dict_type, size, output, sheet, markers_per_row):
|
|
297
|
+
"""
|
|
298
|
+
Generate standalone ArUco PNG markers (use `kit print` for a rigid board).
|
|
299
|
+
|
|
300
|
+
Example: aruco-pose generate 0 1 2 3 4 5 --sheet --output markers/sheet.png
|
|
301
|
+
"""
|
|
302
|
+
logger = setup_logger("generate")
|
|
303
|
+
|
|
304
|
+
try:
|
|
305
|
+
from aruco_pose.utils.marker_generator import (
|
|
306
|
+
generate_aruco_marker,
|
|
307
|
+
generate_marker_sheet,
|
|
308
|
+
)
|
|
309
|
+
|
|
310
|
+
if sheet:
|
|
311
|
+
generate_marker_sheet(
|
|
312
|
+
marker_ids=list(marker_ids),
|
|
313
|
+
dict_type=dict_type,
|
|
314
|
+
markers_per_row=markers_per_row,
|
|
315
|
+
marker_size_px=size,
|
|
316
|
+
output_path=output,
|
|
317
|
+
)
|
|
318
|
+
else:
|
|
319
|
+
for marker_id in marker_ids:
|
|
320
|
+
generate_aruco_marker(
|
|
321
|
+
marker_id=marker_id,
|
|
322
|
+
dict_type=dict_type,
|
|
323
|
+
size_px=size,
|
|
324
|
+
output_dir=output,
|
|
325
|
+
)
|
|
326
|
+
|
|
327
|
+
logger.info("✓ Marker generation complete!")
|
|
328
|
+
|
|
329
|
+
except Exception as e:
|
|
330
|
+
logger.error(f"Generation failed: {e}")
|
|
331
|
+
sys.exit(1)
|
|
332
|
+
|
|
333
|
+
|
|
334
|
+
@cli.command()
|
|
335
|
+
@click.argument("video_path", type=click.Path(exists=True))
|
|
336
|
+
@click.option("--calibration", required=True, help="Path to calibration matrix (.npy)")
|
|
337
|
+
@click.option(
|
|
338
|
+
"--distortion", required=True, help="Path to distortion coefficients (.npy)"
|
|
339
|
+
)
|
|
340
|
+
@click.option("--dict-type", default="DICT_5X5_100", help="ArUco dictionary type")
|
|
341
|
+
@click.option("--marker-size", type=float, default=0.05, help="Marker size in meters")
|
|
342
|
+
@click.option("--output", help="Output analyzed video")
|
|
343
|
+
def analyze(video_path, calibration, distortion, dict_type, marker_size, output):
|
|
344
|
+
"""Analyze a recorded video and generate statistics."""
|
|
345
|
+
logger = setup_logger("analyze")
|
|
346
|
+
|
|
347
|
+
try:
|
|
348
|
+
# Load calibration
|
|
349
|
+
K = np.load(calibration)
|
|
350
|
+
D = np.load(distortion)
|
|
351
|
+
|
|
352
|
+
# Initialize components
|
|
353
|
+
detector = UnifiedArucoDetector(dict_type)
|
|
354
|
+
visualizer = ArucoVisualizer(K, D)
|
|
355
|
+
monitor = PerformanceMonitor()
|
|
356
|
+
|
|
357
|
+
video = VideoHandler(video_path)
|
|
358
|
+
|
|
359
|
+
writer = None
|
|
360
|
+
if output:
|
|
361
|
+
props = video.get_properties()
|
|
362
|
+
fourcc = cv2.VideoWriter_fourcc(*"mp4v")
|
|
363
|
+
writer = cv2.VideoWriter(
|
|
364
|
+
output, fourcc, props["fps"], (props["width"], props["height"])
|
|
365
|
+
)
|
|
366
|
+
|
|
367
|
+
logger.info(f"Analyzing video: {video_path}")
|
|
368
|
+
|
|
369
|
+
while True:
|
|
370
|
+
ret, frame = video.read()
|
|
371
|
+
if not ret:
|
|
372
|
+
break
|
|
373
|
+
|
|
374
|
+
corners, ids, _ = detector.detect(frame)
|
|
375
|
+
n_markers = len(ids) if ids is not None else 0
|
|
376
|
+
monitor.record_detection(n_markers)
|
|
377
|
+
|
|
378
|
+
if writer:
|
|
379
|
+
frame = visualizer.draw_markers(
|
|
380
|
+
frame, corners, ids, draw_axes=True, marker_size=marker_size
|
|
381
|
+
)
|
|
382
|
+
writer.write(frame)
|
|
383
|
+
|
|
384
|
+
video.release()
|
|
385
|
+
if writer:
|
|
386
|
+
writer.release()
|
|
387
|
+
logger.info(f"Saved analyzed video: {output}")
|
|
388
|
+
|
|
389
|
+
logger.info("\n" + monitor.get_summary())
|
|
390
|
+
logger.info("✓ Analysis complete!")
|
|
391
|
+
|
|
392
|
+
except Exception as e:
|
|
393
|
+
logger.error(f"Analysis failed: {e}")
|
|
394
|
+
sys.exit(1)
|
|
395
|
+
|
|
396
|
+
|
|
397
|
+
from .kit_cli import kit
|
|
398
|
+
|
|
399
|
+
cli.add_command(kit)
|
|
400
|
+
|
|
401
|
+
|
|
402
|
+
if __name__ == "__main__":
|
|
403
|
+
cli()
|
|
@@ -0,0 +1,191 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Unified ArUco detector that handles different OpenCV versions seamlessly.
|
|
3
|
+
"""
|
|
4
|
+
|
|
5
|
+
import cv2
|
|
6
|
+
import numpy as np
|
|
7
|
+
from packaging.version import Version
|
|
8
|
+
import logging
|
|
9
|
+
|
|
10
|
+
logger = logging.getLogger(__name__)
|
|
11
|
+
|
|
12
|
+
# Dictionary mapping of ArUco names to OpenCV dictionary IDs
|
|
13
|
+
ARUCO_DICT = {
|
|
14
|
+
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
|
|
15
|
+
"DICT_4X4_100": cv2.aruco.DICT_4X4_100,
|
|
16
|
+
"DICT_4X4_250": cv2.aruco.DICT_4X4_250,
|
|
17
|
+
"DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
|
|
18
|
+
"DICT_5X5_50": cv2.aruco.DICT_5X5_50,
|
|
19
|
+
"DICT_5X5_100": cv2.aruco.DICT_5X5_100,
|
|
20
|
+
"DICT_5X5_250": cv2.aruco.DICT_5X5_250,
|
|
21
|
+
"DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
|
|
22
|
+
"DICT_6X6_50": cv2.aruco.DICT_6X6_50,
|
|
23
|
+
"DICT_6X6_100": cv2.aruco.DICT_6X6_100,
|
|
24
|
+
"DICT_6X6_250": cv2.aruco.DICT_6X6_250,
|
|
25
|
+
"DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
|
|
26
|
+
"DICT_7X7_50": cv2.aruco.DICT_7X7_50,
|
|
27
|
+
"DICT_7X7_100": cv2.aruco.DICT_7X7_100,
|
|
28
|
+
"DICT_7X7_250": cv2.aruco.DICT_7X7_250,
|
|
29
|
+
"DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
|
|
30
|
+
"DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
|
|
31
|
+
"DICT_APRILTAG_16h5": cv2.aruco.DICT_APRILTAG_16h5,
|
|
32
|
+
"DICT_APRILTAG_25h9": cv2.aruco.DICT_APRILTAG_25h9,
|
|
33
|
+
"DICT_APRILTAG_36h10": cv2.aruco.DICT_APRILTAG_36h10,
|
|
34
|
+
"DICT_APRILTAG_36h11": cv2.aruco.DICT_APRILTAG_36h11
|
|
35
|
+
}
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
class UnifiedArucoDetector:
|
|
39
|
+
"""
|
|
40
|
+
Handles ArUco marker detection across different OpenCV versions.
|
|
41
|
+
|
|
42
|
+
Automatically detects OpenCV version and uses appropriate API.
|
|
43
|
+
Supports both legacy (< 4.7.0) and modern OpenCV versions.
|
|
44
|
+
|
|
45
|
+
Args:
|
|
46
|
+
aruco_dict_type: ArUco dictionary type (string or OpenCV constant)
|
|
47
|
+
params: Detection parameters (optional)
|
|
48
|
+
|
|
49
|
+
Example:
|
|
50
|
+
>>> detector = UnifiedArucoDetector("DICT_5X5_100")
|
|
51
|
+
>>> corners, ids, rejected = detector.detect(image)
|
|
52
|
+
"""
|
|
53
|
+
|
|
54
|
+
def __init__(self, aruco_dict_type="DICT_5X5_100", params=None):
|
|
55
|
+
"""
|
|
56
|
+
Initialize the detector with specified dictionary type.
|
|
57
|
+
|
|
58
|
+
Args:
|
|
59
|
+
aruco_dict_type: String name or OpenCV constant for ArUco dictionary
|
|
60
|
+
params: Optional DetectorParameters for customization
|
|
61
|
+
"""
|
|
62
|
+
if isinstance(aruco_dict_type, str):
|
|
63
|
+
if aruco_dict_type not in ARUCO_DICT:
|
|
64
|
+
raise ValueError(
|
|
65
|
+
f"Invalid ArUco dictionary type: {aruco_dict_type}. "
|
|
66
|
+
f"Must be one of {list(ARUCO_DICT.keys())}"
|
|
67
|
+
)
|
|
68
|
+
self.dict_type = ARUCO_DICT[aruco_dict_type]
|
|
69
|
+
self.dict_name = aruco_dict_type
|
|
70
|
+
else:
|
|
71
|
+
self.dict_type = aruco_dict_type
|
|
72
|
+
self.dict_name = "custom"
|
|
73
|
+
|
|
74
|
+
self._setup_detector(params)
|
|
75
|
+
logger.info(f"Initialized ArUco detector with {self.dict_name} "
|
|
76
|
+
f"(OpenCV {cv2.__version__}, legacy={self.use_legacy})")
|
|
77
|
+
|
|
78
|
+
def _setup_detector(self, params):
|
|
79
|
+
"""
|
|
80
|
+
Auto-detect OpenCV version and setup appropriate detector.
|
|
81
|
+
|
|
82
|
+
Args:
|
|
83
|
+
params: Optional detection parameters
|
|
84
|
+
"""
|
|
85
|
+
cv_version = Version(cv2.__version__)
|
|
86
|
+
|
|
87
|
+
if cv_version >= Version("4.7.0"):
|
|
88
|
+
# Modern OpenCV API (>= 4.7.0)
|
|
89
|
+
self.aruco_dict = cv2.aruco.getPredefinedDictionary(self.dict_type)
|
|
90
|
+
self.params = params if params is not None else cv2.aruco.DetectorParameters()
|
|
91
|
+
self.detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.params)
|
|
92
|
+
self.use_legacy = False
|
|
93
|
+
else:
|
|
94
|
+
# Legacy OpenCV API (< 4.7.0)
|
|
95
|
+
self.aruco_dict = cv2.aruco.Dictionary_get(self.dict_type)
|
|
96
|
+
self.params = params if params is not None else cv2.aruco.DetectorParameters_create()
|
|
97
|
+
self.detector = None
|
|
98
|
+
self.use_legacy = True
|
|
99
|
+
|
|
100
|
+
def detect(self, image, convert_to_gray=True):
|
|
101
|
+
"""
|
|
102
|
+
Detect ArUco markers in an image.
|
|
103
|
+
|
|
104
|
+
Args:
|
|
105
|
+
image: Input image (BGR or grayscale)
|
|
106
|
+
convert_to_gray: Whether to convert to grayscale (default: True)
|
|
107
|
+
|
|
108
|
+
Returns:
|
|
109
|
+
Tuple of (corners, ids, rejected_candidates)
|
|
110
|
+
- corners: List of detected marker corners
|
|
111
|
+
- ids: Array of marker IDs
|
|
112
|
+
- rejected_candidates: List of rejected marker candidates
|
|
113
|
+
|
|
114
|
+
Raises:
|
|
115
|
+
ValueError: If image is None or empty
|
|
116
|
+
"""
|
|
117
|
+
if image is None or image.size == 0:
|
|
118
|
+
raise ValueError("Input image is None or empty")
|
|
119
|
+
|
|
120
|
+
# Convert to grayscale if needed
|
|
121
|
+
if convert_to_gray and len(image.shape) == 3:
|
|
122
|
+
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
|
|
123
|
+
else:
|
|
124
|
+
gray = image
|
|
125
|
+
|
|
126
|
+
# Detect markers using appropriate API
|
|
127
|
+
if self.use_legacy:
|
|
128
|
+
corners, ids, rejected = cv2.aruco.detectMarkers(
|
|
129
|
+
gray, self.aruco_dict, parameters=self.params
|
|
130
|
+
)
|
|
131
|
+
else:
|
|
132
|
+
corners, ids, rejected = self.detector.detectMarkers(gray)
|
|
133
|
+
|
|
134
|
+
# Log detection results
|
|
135
|
+
n_detected = len(ids) if ids is not None else 0
|
|
136
|
+
logger.debug(f"Detected {n_detected} markers, {len(rejected)} rejected")
|
|
137
|
+
|
|
138
|
+
return corners, ids, rejected
|
|
139
|
+
|
|
140
|
+
def get_marker_size(self, corners):
|
|
141
|
+
"""
|
|
142
|
+
Calculate the average size of detected markers in pixels.
|
|
143
|
+
|
|
144
|
+
Args:
|
|
145
|
+
corners: Marker corners from detection
|
|
146
|
+
|
|
147
|
+
Returns:
|
|
148
|
+
Average marker size in pixels
|
|
149
|
+
"""
|
|
150
|
+
if not corners:
|
|
151
|
+
return 0.0
|
|
152
|
+
|
|
153
|
+
sizes = []
|
|
154
|
+
for corner in corners:
|
|
155
|
+
pts = corner[0]
|
|
156
|
+
# Calculate average edge length
|
|
157
|
+
edge_lengths = [
|
|
158
|
+
np.linalg.norm(pts[i] - pts[(i+1) % 4])
|
|
159
|
+
for i in range(4)
|
|
160
|
+
]
|
|
161
|
+
sizes.append(np.mean(edge_lengths))
|
|
162
|
+
|
|
163
|
+
return np.mean(sizes)
|
|
164
|
+
|
|
165
|
+
def update_parameters(self, **kwargs):
|
|
166
|
+
"""
|
|
167
|
+
Update detector parameters dynamically.
|
|
168
|
+
|
|
169
|
+
Args:
|
|
170
|
+
**kwargs: Parameter names and values to update
|
|
171
|
+
|
|
172
|
+
Example:
|
|
173
|
+
>>> detector.update_parameters(
|
|
174
|
+
... adaptiveThreshConstant=7,
|
|
175
|
+
... minMarkerPerimeterRate=0.03
|
|
176
|
+
... )
|
|
177
|
+
"""
|
|
178
|
+
for key, value in kwargs.items():
|
|
179
|
+
if hasattr(self.params, key):
|
|
180
|
+
setattr(self.params, key, value)
|
|
181
|
+
logger.debug(f"Updated parameter {key} = {value}")
|
|
182
|
+
else:
|
|
183
|
+
logger.warning(f"Parameter {key} not found in DetectorParameters")
|
|
184
|
+
|
|
185
|
+
# Recreate detector with updated parameters if using modern API
|
|
186
|
+
if not self.use_legacy:
|
|
187
|
+
self.detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.params)
|
|
188
|
+
|
|
189
|
+
def __repr__(self):
|
|
190
|
+
return (f"UnifiedArucoDetector(dict={self.dict_name}, "
|
|
191
|
+
f"opencv_version={cv2.__version__}, legacy={self.use_legacy})")
|