Add AprilTag calibration, assets, and tracking demo

.gitignore

@@ -0,0 +1,3 @@
+__pycache__/
+*.py[cod]
+*.mp4

analysis_output/apriltag_analysis_report.json

@@ -0,0 +1,68 @@
+{
+  "video_path": "C:\\devel\\slam_demo\\VID_20260626_142931.mp4",
+  "calibration_path": "C:\\devel\\slam_demo\\calibration_output\\camera_calibration.npz",
+  "tag_size_m": 0.2,
+  "precise_distance_m": 3.0,
+  "video": {
+    "fps": 29.490109925345436,
+    "frame_count": 3965,
+    "width": 1080,
+    "height": 1920,
+    "duration_sec": 134.45185555555557
+  },
+  "detections_total": 2043,
+  "frames_with_tags": 2042,
+  "frames_with_tags_ratio": 0.5150063051702396,
+  "detected_tag_ids": [
+    1,
+    10,
+    21,
+    31
+  ],
+  "distance_global_min_m": 0.6724304161465295,
+  "distance_global_max_m": 29.05393296811889,
+  "tag_summary": {
+    "1": {
+      "count": 649,
+      "distance_min_m": 0.6724304161465295,
+      "distance_max_m": 29.05393296811889,
+      "side_px_max": 1001.5821990966797,
+      "precise_track_frames": 265,
+      "first_seen_sec": 1.5937546560179348,
+      "last_seen_sec": 134.41794588202328
+    },
+    "10": {
+      "count": 541,
+      "distance_min_m": 0.6936149816954829,
+      "distance_max_m": 10.514124997867455,
+      "side_px_max": 962.2133636474609,
+      "precise_track_frames": 287,
+      "first_seen_sec": 24.58451331091495,
+      "last_seen_sec": 43.09919505954883
+    },
+    "21": {
+      "count": 492,
+      "distance_min_m": 1.0321140671608207,
+      "distance_max_m": 9.818515071203956,
+      "side_px_max": 658.1097869873047,
+      "precise_track_frames": 306,
+      "first_seen_sec": 25.228797108028584,
+      "last_seen_sec": 74.22827536219701
+    },
+    "31": {
+      "count": 361,
+      "distance_min_m": 1.4224853614043138,
+      "distance_max_m": 2.9749145431912978,
+      "side_px_max": 486.1072769165039,
+      "precise_track_frames": 361,
+      "first_seen_sec": 94.91317621689787,
+      "last_seen_sec": 107.32411672971837
+    }
+  },
+  "best_frame_previews": {
+    "1": "C:\\devel\\slam_demo\\analysis_output\\best_tag_01.jpg",
+    "10": "C:\\devel\\slam_demo\\analysis_output\\best_tag_10.jpg",
+    "21": "C:\\devel\\slam_demo\\analysis_output\\best_tag_21.jpg",
+    "31": "C:\\devel\\slam_demo\\analysis_output\\best_tag_31.jpg"
+  }
+}

analyze_apriltag_video.py

@@ -0,0 +1,286 @@
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import math
+from dataclasses import asdict, dataclass
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+
+DICT_ID = cv2.aruco.DICT_APRILTAG_36h11
+
+
+@dataclass
+class DetectionRow:
+    frame_index: int
+    time_sec: float
+    tag_id: int
+    center_x_px: float
+    center_y_px: float
+    side_px: float
+    distance_m: float
+    x_m: float
+    y_m: float
+    z_m: float
+    yaw_deg: float
+    pitch_deg: float
+    roll_deg: float
+    reproj_err_px: float
+
+
+def load_calibration(path: Path) -> tuple[np.ndarray, np.ndarray]:
+    data = np.load(path)
+    return data["camera_matrix"], data["dist_coeffs"]
+
+
+def build_object_points(tag_size_m: float) -> np.ndarray:
+    half = tag_size_m / 2.0
+    return np.array(
+        [
+            [-half, half, 0.0],
+            [half, half, 0.0],
+            [half, -half, 0.0],
+            [-half, -half, 0.0],
+        ],
+        dtype=np.float32,
+    )
+
+
+def pose_to_euler_deg(rvec: np.ndarray) -> tuple[float, float, float]:
+    rot, _ = cv2.Rodrigues(rvec)
+    yaw = math.degrees(math.atan2(rot[1, 0], rot[0, 0]))
+    pitch = math.degrees(math.atan2(-rot[2, 0], math.sqrt(rot[2, 1] ** 2 + rot[2, 2] ** 2)))
+    roll = math.degrees(math.atan2(rot[2, 1], rot[2, 2]))
+    return yaw, pitch, roll
+
+
+def estimate_side_px(points: np.ndarray) -> float:
+    lengths = []
+    for i in range(4):
+        p0 = points[i]
+        p1 = points[(i + 1) % 4]
+        lengths.append(float(np.linalg.norm(p1 - p0)))
+    return float(sum(lengths) / len(lengths))
+
+
+def reprojection_error(
+    object_points: np.ndarray,
+    image_points: np.ndarray,
+    rvec: np.ndarray,
+    tvec: np.ndarray,
+    camera_matrix: np.ndarray,
+    dist_coeffs: np.ndarray,
+) -> float:
+    projected, _ = cv2.projectPoints(object_points, rvec, tvec, camera_matrix, dist_coeffs)
+    projected = projected.reshape(-1, 2)
+    err = np.linalg.norm(projected - image_points, axis=1)
+    return float(np.mean(err))
+
+
+def detect_video(
+    video_path: Path,
+    calibration_path: Path,
+    output_dir: Path,
+    tag_size_m: float,
+    precise_distance_m: float,
+) -> dict:
+    output_dir.mkdir(parents=True, exist_ok=True)
+    camera_matrix, dist_coeffs = load_calibration(calibration_path)
+    object_points = build_object_points(tag_size_m)
+
+    dictionary = cv2.aruco.getPredefinedDictionary(DICT_ID)
+    parameters = cv2.aruco.DetectorParameters()
+    detector = cv2.aruco.ArucoDetector(dictionary, parameters)
+
+    cap = cv2.VideoCapture(str(video_path))
+    if not cap.isOpened():
+        raise RuntimeError(f"Cannot open video: {video_path}")
+
+    fps = float(cap.get(cv2.CAP_PROP_FPS))
+    frame_count = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
+    width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+    height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+    duration_sec = frame_count / fps if fps else 0.0
+
+    detections: list[DetectionRow] = []
+    tag_summary: dict[int, dict] = {}
+    best_frames: dict[int, tuple[DetectionRow, np.ndarray]] = {}
+    frames_with_tags = 0
+
+    frame_index = 0
+    while True:
+        ok, frame = cap.read()
+        if not ok:
+            break
+
+        corners, ids, _ = detector.detectMarkers(frame)
+        if ids is not None and len(ids) > 0:
+            frames_with_tags += 1
+
+        if ids is not None:
+            for marker_corners, marker_id_arr in zip(corners, ids):
+                tag_id = int(marker_id_arr[0])
+                image_points = marker_corners.reshape(4, 2).astype(np.float32)
+                ok_pnp, rvec, tvec = cv2.solvePnP(
+                    object_points,
+                    image_points,
+                    camera_matrix,
+                    dist_coeffs,
+                    flags=cv2.SOLVEPNP_IPPE_SQUARE,
+                )
+                if not ok_pnp:
+                    continue
+
+                x_m = float(tvec[0, 0])
+                y_m = float(tvec[1, 0])
+                z_m = float(tvec[2, 0])
+                distance_m = float(np.linalg.norm(tvec))
+                yaw_deg, pitch_deg, roll_deg = pose_to_euler_deg(rvec)
+                side_px = estimate_side_px(image_points)
+                center = image_points.mean(axis=0)
+                reproj_err_px = reprojection_error(
+                    object_points, image_points, rvec, tvec, camera_matrix, dist_coeffs
+                )
+
+                row = DetectionRow(
+                    frame_index=frame_index,
+                    time_sec=frame_index / fps if fps else 0.0,
+                    tag_id=tag_id,
+                    center_x_px=float(center[0]),
+                    center_y_px=float(center[1]),
+                    side_px=side_px,
+                    distance_m=distance_m,
+                    x_m=x_m,
+                    y_m=y_m,
+                    z_m=z_m,
+                    yaw_deg=yaw_deg,
+                    pitch_deg=pitch_deg,
+                    roll_deg=roll_deg,
+                    reproj_err_px=reproj_err_px,
+                )
+                detections.append(row)
+
+                summary = tag_summary.setdefault(
+                    tag_id,
+                    {
+                        "count": 0,
+                        "distance_min_m": float("inf"),
+                        "distance_max_m": 0.0,
+                        "side_px_max": 0.0,
+                        "precise_track_frames": 0,
+                        "first_seen_sec": row.time_sec,
+                        "last_seen_sec": row.time_sec,
+                    },
+                )
+                summary["count"] += 1
+                summary["distance_min_m"] = min(summary["distance_min_m"], distance_m)
+                summary["distance_max_m"] = max(summary["distance_max_m"], distance_m)
+                summary["side_px_max"] = max(summary["side_px_max"], side_px)
+                if distance_m <= precise_distance_m:
+                    summary["precise_track_frames"] += 1
+                summary["last_seen_sec"] = row.time_sec
+
+                current_best = best_frames.get(tag_id)
+                if current_best is None or row.side_px > current_best[0].side_px:
+                    best_frames[tag_id] = (row, frame.copy())
+
+        frame_index += 1
+
+    cap.release()
+
+    csv_path = output_dir / "apriltag_detections.csv"
+    with csv_path.open("w", newline="", encoding="utf-8") as fh:
+        writer = csv.DictWriter(fh, fieldnames=list(asdict(detections[0]).keys()) if detections else list(DetectionRow.__dataclass_fields__.keys()))
+        writer.writeheader()
+        for row in detections:
+            writer.writerow(asdict(row))
+
+    preview_paths = {}
+    for tag_id, (row, frame) in best_frames.items():
+        corners, ids, _ = detector.detectMarkers(frame)
+        annotated = frame.copy()
+        if ids is not None:
+            cv2.aruco.drawDetectedMarkers(annotated, corners, ids)
+        text = (
+            f"ID {tag_id}  t={row.time_sec:.1f}s  d={row.distance_m:.2f}m  "
+            f"x={row.x_m:+.2f} y={row.y_m:+.2f} z={row.z_m:+.2f}  yaw={row.yaw_deg:+.1f}"
+        )
+        cv2.putText(annotated, text, (30, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2, cv2.LINE_AA)
+        preview_path = output_dir / f"best_tag_{tag_id:02d}.jpg"
+        cv2.imwrite(str(preview_path), annotated)
+        preview_paths[tag_id] = str(preview_path)
+
+    if detections:
+        all_distances = [row.distance_m for row in detections]
+        report = {
+            "video_path": str(video_path),
+            "calibration_path": str(calibration_path),
+            "tag_size_m": tag_size_m,
+            "precise_distance_m": precise_distance_m,
+            "video": {
+                "fps": fps,
+                "frame_count": frame_count,
+                "width": width,
+                "height": height,
+                "duration_sec": duration_sec,
+            },
+            "detections_total": len(detections),
+            "frames_with_tags": frames_with_tags,
+            "frames_with_tags_ratio": frames_with_tags / frame_count if frame_count else 0.0,
+            "detected_tag_ids": sorted(int(k) for k in tag_summary.keys()),
+            "distance_global_min_m": min(all_distances),
+            "distance_global_max_m": max(all_distances),
+            "tag_summary": tag_summary,
+            "best_frame_previews": preview_paths,
+        }
+    else:
+        report = {
+            "video_path": str(video_path),
+            "calibration_path": str(calibration_path),
+            "tag_size_m": tag_size_m,
+            "precise_distance_m": precise_distance_m,
+            "video": {
+                "fps": fps,
+                "frame_count": frame_count,
+                "width": width,
+                "height": height,
+                "duration_sec": duration_sec,
+            },
+            "detections_total": 0,
+            "frames_with_tags": 0,
+            "frames_with_tags_ratio": 0.0,
+            "detected_tag_ids": [],
+            "tag_summary": {},
+            "best_frame_previews": {},
+        }
+
+    report_path = output_dir / "apriltag_analysis_report.json"
+    report_path.write_text(json.dumps(report, indent=2), encoding="utf-8")
+    return report
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--video", required=True)
+    parser.add_argument("--calibration", required=True)
+    parser.add_argument("--output-dir", required=True)
+    parser.add_argument("--tag-size-m", type=float, default=0.20)
+    parser.add_argument("--precise-distance-m", type=float, default=3.0)
+    args = parser.parse_args()
+
+    report = detect_video(
+        Path(args.video),
+        Path(args.calibration),
+        Path(args.output_dir),
+        args.tag_size_m,
+        args.precise_distance_m,
+    )
+    print(json.dumps(report, indent=2))
+
+
+if __name__ == "__main__":
+    main()

apriltag_tracking_demo.py

@@ -0,0 +1,385 @@
+from __future__ import annotations
+
+import argparse
+import math
+from collections import deque
+from dataclasses import dataclass
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+
+DICT_ID = cv2.aruco.DICT_APRILTAG_36h11
+
+
+@dataclass
+class PoseEstimate:
+    tag_id: int
+    frame_index: int
+    time_sec: float
+    corners: np.ndarray
+    center_x_px: float
+    center_y_px: float
+    side_px: float
+    x_m: float
+    y_m: float
+    z_m: float
+    distance_m: float
+    yaw_deg: float
+    pitch_deg: float
+    roll_deg: float
+
+
+def load_calibration(path: Path) -> tuple[np.ndarray, np.ndarray]:
+    data = np.load(path)
+    return data["camera_matrix"], data["dist_coeffs"]
+
+
+def build_object_points(tag_size_m: float) -> np.ndarray:
+    half = tag_size_m / 2.0
+    return np.array(
+        [
+            [-half, half, 0.0],
+            [half, half, 0.0],
+            [half, -half, 0.0],
+            [-half, -half, 0.0],
+        ],
+        dtype=np.float32,
+    )
+
+
+def pose_to_euler_deg(rvec: np.ndarray) -> tuple[float, float, float]:
+    rot, _ = cv2.Rodrigues(rvec)
+    yaw = math.degrees(math.atan2(rot[1, 0], rot[0, 0]))
+    pitch = math.degrees(math.atan2(-rot[2, 0], math.sqrt(rot[2, 1] ** 2 + rot[2, 2] ** 2)))
+    roll = math.degrees(math.atan2(rot[2, 1], rot[2, 2]))
+    return yaw, pitch, roll
+
+
+def estimate_side_px(points: np.ndarray) -> float:
+    lengths = []
+    for i in range(4):
+        p0 = points[i]
+        p1 = points[(i + 1) % 4]
+        lengths.append(float(np.linalg.norm(p1 - p0)))
+    return float(sum(lengths) / len(lengths))
+
+
+def detect_primary_pose(
+    frame: np.ndarray,
+    frame_index: int,
+    time_sec: float,
+    detector: cv2.aruco.ArucoDetector,
+    object_points: np.ndarray,
+    camera_matrix: np.ndarray,
+    dist_coeffs: np.ndarray,
+) -> tuple[PoseEstimate | None, list[PoseEstimate]]:
+    corners, ids, _ = detector.detectMarkers(frame)
+    if ids is None or len(ids) == 0:
+        return None, []
+
+    detections: list[PoseEstimate] = []
+    for marker_corners, marker_id_arr in zip(corners, ids):
+        tag_id = int(marker_id_arr[0])
+        image_points = marker_corners.reshape(4, 2).astype(np.float32)
+        ok_pnp, rvec, tvec = cv2.solvePnP(
+            object_points,
+            image_points,
+            camera_matrix,
+            dist_coeffs,
+            flags=cv2.SOLVEPNP_IPPE_SQUARE,
+        )
+        if not ok_pnp:
+            continue
+        x_m = float(tvec[0, 0])
+        y_m = float(tvec[1, 0])
+        z_m = float(tvec[2, 0])
+        distance_m = float(np.linalg.norm(tvec))
+        yaw_deg, pitch_deg, roll_deg = pose_to_euler_deg(rvec)
+        center = image_points.mean(axis=0)
+        detections.append(
+            PoseEstimate(
+                tag_id=tag_id,
+                frame_index=frame_index,
+                time_sec=time_sec,
+                corners=image_points,
+                center_x_px=float(center[0]),
+                center_y_px=float(center[1]),
+                side_px=estimate_side_px(image_points),
+                x_m=x_m,
+                y_m=y_m,
+                z_m=z_m,
+                distance_m=distance_m,
+                yaw_deg=yaw_deg,
+                pitch_deg=pitch_deg,
+                roll_deg=roll_deg,
+            )
+        )
+
+    if not detections:
+        return None, []
+
+    detections.sort(key=lambda d: (d.side_px, -d.distance_m), reverse=True)
+    return detections[0], detections
+
+
+def tracking_state(pose: PoseEstimate | None, precise_distance_m: float) -> str:
+    if pose is None:
+        return "SEARCH"
+    if pose.distance_m > precise_distance_m:
+        return "APPROACH"
+    if abs(pose.x_m) < 0.08 and abs(pose.y_m) < 0.08 and abs(pose.yaw_deg) < 4.0:
+        return "LOCK"
+    return "PRECISE_TRACK"
+
+
+def draw_pose_axes(panel: np.ndarray, pose: PoseEstimate | None) -> None:
+    h, w = panel.shape[:2]
+    cx, cy = w // 2, h // 2
+    cv2.line(panel, (cx - 180, cy), (cx + 180, cy), (70, 70, 70), 1, cv2.LINE_AA)
+    cv2.line(panel, (cx, cy - 180), (cx, cy + 180), (70, 70, 70), 1, cv2.LINE_AA)
+    cv2.circle(panel, (cx, cy), 4, (200, 200, 200), -1, cv2.LINE_AA)
+    cv2.putText(panel, "X laterale", (cx + 20, cy - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (170, 170, 170), 1, cv2.LINE_AA)
+    cv2.putText(panel, "Y verticale", (cx + 20, cy + 22), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (170, 170, 170), 1, cv2.LINE_AA)
+    if pose is None:
+        return
+    px = int(np.clip(cx + pose.x_m * 220, 40, w - 40))
+    py = int(np.clip(cy + pose.y_m * 220, 40, h - 40))
+    cv2.circle(panel, (px, py), 12, (0, 220, 255), -1, cv2.LINE_AA)
+    cv2.line(panel, (cx, cy), (px, py), (0, 220, 255), 2, cv2.LINE_AA)
+
+
+def draw_tracking_panel(panel: np.ndarray, pose: PoseEstimate | None, state: str, precise_distance_m: float) -> None:
+    panel[:] = (24, 24, 28)
+    cv2.putText(panel, "AUTOTRACKING", (24, 42), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (240, 240, 240), 2, cv2.LINE_AA)
+    color = {
+        "SEARCH": (110, 110, 110),
+        "APPROACH": (0, 200, 255),
+        "PRECISE_TRACK": (0, 220, 0),
+        "LOCK": (255, 200, 0),
+    }[state]
+    cv2.rectangle(panel, (24, 62), (250, 110), color, -1)
+    cv2.putText(panel, state, (38, 95), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (10, 10, 10), 2, cv2.LINE_AA)
+
+    if pose is None:
+        cv2.putText(panel, "Nessun tag visibile", (24, 156), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (220, 220, 220), 2, cv2.LINE_AA)
+        draw_pose_axes(panel, None)
+        return
+
+    lines = [
+        f"Tag attivo: {pose.tag_id}",
+        f"Distanza: {pose.distance_m:.2f} m",
+        f"Tracking preciso sotto: {precise_distance_m:.2f} m",
+        f"Offset X: {pose.x_m:+.2f} m",
+        f"Offset Y: {pose.y_m:+.2f} m",
+        f"Profondita Z: {pose.z_m:+.2f} m",
+        f"Yaw: {pose.yaw_deg:+.1f} deg",
+        f"Pitch: {pose.pitch_deg:+.1f} deg",
+        f"Roll: {pose.roll_deg:+.1f} deg",
+    ]
+    y = 156
+    for line in lines:
+        cv2.putText(panel, line, (24, y), cv2.FONT_HERSHEY_SIMPLEX, 0.72, (220, 220, 220), 2, cv2.LINE_AA)
+        y += 38
+
+    draw_pose_axes(panel, pose)
+
+
+def fmt_motion(value: float, pos_label: str, neg_label: str, unit: str = "m/s") -> str:
+    if abs(value) < 1e-3:
+        return f"FERMO 0.00 {unit}"
+    label = pos_label if value > 0 else neg_label
+    return f"{label} {abs(value):.2f} {unit}"
+
+
+def draw_motion_bar(panel: np.ndarray, y: int, label: str, value: float, scale: float, color: tuple[int, int, int]) -> None:
+    left = 220
+    center = 420
+    width = 180
+    cv2.putText(panel, label, (24, y + 8), cv2.FONT_HERSHEY_SIMPLEX, 0.72, (220, 220, 220), 2, cv2.LINE_AA)
+    cv2.line(panel, (center - width, y), (center + width, y), (80, 80, 80), 2, cv2.LINE_AA)
+    cv2.line(panel, (center, y - 16), (center, y + 16), (170, 170, 170), 2, cv2.LINE_AA)
+    px = int(np.clip(center + (value / scale) * width, center - width, center + width))
+    cv2.circle(panel, (px, y), 10, color, -1, cv2.LINE_AA)
+
+
+def draw_motion_panel(panel: np.ndarray, motion: dict, active_tag_id: int | None) -> None:
+    panel[:] = (20, 22, 26)
+    cv2.putText(panel, "COMANDI APPENA ESEGUITI", (24, 42), cv2.FONT_HERSHEY_SIMPLEX, 0.95, (240, 240, 240), 2, cv2.LINE_AA)
+    tag_text = f"Tag di riferimento: {active_tag_id}" if active_tag_id is not None else "Tag di riferimento: nessuno"
+    cv2.putText(panel, tag_text, (24, 82), cv2.FONT_HERSHEY_SIMPLEX, 0.72, (200, 200, 200), 2, cv2.LINE_AA)
+    cv2.putText(panel, "I valori descrivono cosa il drone ha appena fatto fra due pose successive.", (24, 116), cv2.FONT_HERSHEY_SIMPLEX, 0.58, (170, 170, 170), 1, cv2.LINE_AA)
+
+    lines = [
+        ("Longitudinale", motion["forward_mps"], "AVANTI", "INDIETRO", 0.8, (0, 220, 255)),
+        ("Laterale", motion["right_mps"], "DESTRA", "SINISTRA", 0.6, (255, 180, 0)),
+        ("Verticale", motion["up_mps"], "SU", "GIU", 0.4, (120, 220, 120)),
+        ("Yaw", motion["yaw_rate_dps"], "RUOTA DX", "RUOTA SX", 20.0, (220, 120, 220)),
+    ]
+    y = 190
+    for label, value, pos_label, neg_label, scale, color in lines:
+        text = fmt_motion(value, pos_label, neg_label, "deg/s" if label == "Yaw" else "m/s")
+        cv2.putText(panel, f"{label}: {text}", (24, y - 22), cv2.FONT_HERSHEY_SIMPLEX, 0.66, (225, 225, 225), 2, cv2.LINE_AA)
+        draw_motion_bar(panel, y, "", value, scale, color)
+        y += 105
+
+
+def annotate_video_frame(frame: np.ndarray, detections: list[PoseEstimate], primary: PoseEstimate | None, state: str) -> np.ndarray:
+    out = frame.copy()
+    if detections:
+        corners = [d.corners.reshape(1, 4, 2).astype(np.float32) for d in detections]
+        ids = np.array([[d.tag_id] for d in detections], dtype=np.int32)
+        cv2.aruco.drawDetectedMarkers(out, corners, ids)
+
+    if primary is not None:
+        cv2.putText(
+            out,
+            f"ACTIVE TAG {primary.tag_id} | d={primary.distance_m:.2f}m | x={primary.x_m:+.2f} y={primary.y_m:+.2f} z={primary.z_m:+.2f} | yaw={primary.yaw_deg:+.1f}",
+            (20, 40),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.75,
+            (0, 255, 0),
+            2,
+            cv2.LINE_AA,
+        )
+    cv2.putText(
+        out,
+        f"STATE {state}",
+        (20, 76),
+        cv2.FONT_HERSHEY_SIMPLEX,
+        0.8,
+        (0, 220, 255) if state != "LOCK" else (0, 255, 255),
+        2,
+        cv2.LINE_AA,
+    )
+    return out
+
+
+def run_demo(
+    video_path: Path,
+    calibration_path: Path,
+    tag_size_m: float,
+    precise_distance_m: float,
+    write_composite: Path | None,
+    max_frames: int | None,
+    headless: bool,
+) -> None:
+    camera_matrix, dist_coeffs = load_calibration(calibration_path)
+    object_points = build_object_points(tag_size_m)
+    detector = cv2.aruco.ArucoDetector(
+        cv2.aruco.getPredefinedDictionary(DICT_ID),
+        cv2.aruco.DetectorParameters(),
+    )
+
+    cap = cv2.VideoCapture(str(video_path))
+    if not cap.isOpened():
+        raise RuntimeError(f"Cannot open video: {video_path}")
+
+    fps = float(cap.get(cv2.CAP_PROP_FPS))
+    width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+    height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+
+    track_size = (700, 620)
+    motion_size = (840, 620)
+    writer = None
+    if write_composite is not None:
+        fourcc = cv2.VideoWriter_fourcc(*"mp4v")
+        writer = cv2.VideoWriter(str(write_composite), fourcc, fps if fps > 0 else 25.0, (width + track_size[0] + motion_size[0], max(height, track_size[1], motion_size[1])))
+
+    prev_pose: PoseEstimate | None = None
+    smoothed_motion = {
+        "forward_mps": 0.0,
+        "right_mps": 0.0,
+        "up_mps": 0.0,
+        "yaw_rate_dps": 0.0,
+    }
+    alpha = 0.22
+    frame_index = 0
+
+    while True:
+        ok, frame = cap.read()
+        if not ok:
+            break
+        if max_frames is not None and frame_index >= max_frames:
+            break
+
+        time_sec = frame_index / fps if fps else 0.0
+        primary, detections = detect_primary_pose(
+            frame, frame_index, time_sec, detector, object_points, camera_matrix, dist_coeffs
+        )
+        state = tracking_state(primary, precise_distance_m)
+
+        if primary is not None and prev_pose is not None and prev_pose.tag_id == primary.tag_id:
+            dt = max(primary.time_sec - prev_pose.time_sec, 1e-6)
+            instant = {
+                "forward_mps": (prev_pose.z_m - primary.z_m) / dt,
+                "right_mps": (prev_pose.x_m - primary.x_m) / dt,
+                "up_mps": (primary.y_m - prev_pose.y_m) / dt,
+                "yaw_rate_dps": -(primary.yaw_deg - prev_pose.yaw_deg) / dt,
+            }
+            for key, value in instant.items():
+                smoothed_motion[key] = alpha * value + (1.0 - alpha) * smoothed_motion[key]
+        else:
+            for key in smoothed_motion:
+                smoothed_motion[key] *= 0.85
+
+        prev_pose = primary if primary is not None else prev_pose
+
+        video_view = annotate_video_frame(frame, detections, primary, state)
+        tracking_panel = np.zeros((track_size[1], track_size[0], 3), dtype=np.uint8)
+        motion_panel = np.zeros((motion_size[1], motion_size[0], 3), dtype=np.uint8)
+        draw_tracking_panel(tracking_panel, primary, state, precise_distance_m)
+        draw_motion_panel(motion_panel, smoothed_motion, primary.tag_id if primary is not None else None)
+
+        if writer is not None:
+            canvas_h = max(video_view.shape[0], tracking_panel.shape[0], motion_panel.shape[0])
+            canvas_w = video_view.shape[1] + tracking_panel.shape[1] + motion_panel.shape[1]
+            canvas = np.zeros((canvas_h, canvas_w, 3), dtype=np.uint8)
+            canvas[: video_view.shape[0], : video_view.shape[1]] = video_view
+            x1 = video_view.shape[1]
+            canvas[: tracking_panel.shape[0], x1 : x1 + tracking_panel.shape[1]] = tracking_panel
+            x2 = x1 + tracking_panel.shape[1]
+            canvas[: motion_panel.shape[0], x2 : x2 + motion_panel.shape[1]] = motion_panel
+            writer.write(canvas)
+
+        if not headless:
+            cv2.imshow("AprilTag Video", video_view)
+            cv2.imshow("Autotracking", tracking_panel)
+            cv2.imshow("Comandi Simulati", motion_panel)
+            key = cv2.waitKey(1) & 0xFF
+            if key in (27, ord("q")):
+                break
+        frame_index += 1
+
+    cap.release()
+    if writer is not None:
+        writer.release()
+    if not headless:
+        cv2.destroyAllWindows()
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--video", required=True)
+    parser.add_argument("--calibration", required=True)
+    parser.add_argument("--tag-size-m", type=float, default=0.20)
+    parser.add_argument("--precise-distance-m", type=float, default=3.0)
+    parser.add_argument("--write-composite")
+    parser.add_argument("--max-frames", type=int)
+    parser.add_argument("--headless", action="store_true")
+    args = parser.parse_args()
+
+    run_demo(
+        Path(args.video),
+        Path(args.calibration),
+        args.tag_size_m,
+        args.precise_distance_m,
+        Path(args.write_composite) if args.write_composite else None,
+        args.max_frames,
+        args.headless,
+    )
+
+
+if __name__ == "__main__":
+    main()

calibrate_charuco.py

@@ -0,0 +1,174 @@
+import argparse
+import json
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+
+IMAGE_EXTS = {".jpg", ".jpeg", ".png", ".bmp", ".webp"}
+
+
+def parse_args() -> argparse.Namespace:
+    ap = argparse.ArgumentParser(description="Select good ChArUco calibration images and calibrate the camera.")
+    ap.add_argument("--assets-dir", default="assets", help="Directory containing calibration images.")
+    ap.add_argument("--output-dir", default="calibration_output", help="Directory where reports and calibration files are written.")
+    ap.add_argument("--squares-x", type=int, default=7, help="Number of chessboard squares along X.")
+    ap.add_argument("--squares-y", type=int, default=5, help="Number of chessboard squares along Y.")
+    ap.add_argument("--square-length-mm", type=float, default=25.0, help="Square side length in millimeters.")
+    ap.add_argument("--marker-length-mm", type=float, default=18.75, help="Inner marker side length in millimeters.")
+    ap.add_argument("--dictionary", default="DICT_4X4_50", help="OpenCV aruco dictionary name.")
+    ap.add_argument("--min-charuco-corners", type=int, default=12, help="Minimum detected ChArUco corners required to accept an image.")
+    ap.add_argument("--min-markers", type=int, default=8, help="Minimum detected ArUco markers required to accept an image.")
+    ap.add_argument("--min-images", type=int, default=8, help="Minimum accepted images required before calibration.")
+    return ap.parse_args()
+
+
+def get_dictionary(name: str):
+    if not hasattr(cv2.aruco, name):
+        raise ValueError(f"Unknown dictionary: {name}")
+    return cv2.aruco.getPredefinedDictionary(getattr(cv2.aruco, name))
+
+
+def collect_images(assets_dir: Path) -> list[Path]:
+    return sorted([p for p in assets_dir.iterdir() if p.is_file() and p.suffix.lower() in IMAGE_EXTS])
+
+
+def write_yaml(path: Path, camera_matrix: np.ndarray, dist_coeffs: np.ndarray) -> None:
+    fs = cv2.FileStorage(str(path), cv2.FILE_STORAGE_WRITE)
+    fs.write("camera_matrix", camera_matrix)
+    fs.write("dist_coeffs", dist_coeffs)
+    fs.release()
+
+
+def main() -> int:
+    args = parse_args()
+    assets_dir = Path(args.assets_dir)
+    output_dir = Path(args.output_dir)
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    if not assets_dir.exists():
+        raise SystemExit(f"Assets directory not found: {assets_dir}")
+
+    dictionary = get_dictionary(args.dictionary)
+    board = cv2.aruco.CharucoBoard(
+        (args.squares_x, args.squares_y),
+        args.square_length_mm / 1000.0,
+        args.marker_length_mm / 1000.0,
+        dictionary,
+    )
+    detector = cv2.aruco.CharucoDetector(board)
+
+    files = collect_images(assets_dir)
+    if not files:
+        raise SystemExit(f"No images found in {assets_dir}")
+
+    accepted: list[dict] = []
+    rejected: list[dict] = []
+    all_charuco_corners = []
+    all_charuco_ids = []
+    image_size = None
+
+    for path in files:
+        img = cv2.imread(str(path))
+        if img is None:
+            rejected.append({"file": path.name, "reason": "read_fail", "markers": 0, "charuco_corners": 0})
+            continue
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        if image_size is None:
+            image_size = (gray.shape[1], gray.shape[0])
+
+        charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray)
+        markers = 0 if marker_ids is None else len(marker_ids)
+        corners = 0 if charuco_ids is None else len(charuco_ids)
+        record = {
+            "file": path.name,
+            "markers": int(markers),
+            "charuco_corners": int(corners),
+            "width": int(gray.shape[1]),
+            "height": int(gray.shape[0]),
+        }
+
+        if markers >= args.min_markers and corners >= args.min_charuco_corners:
+            accepted.append(record)
+            all_charuco_corners.append(charuco_corners)
+            all_charuco_ids.append(charuco_ids)
+        else:
+            reasons = []
+            if markers < args.min_markers:
+                reasons.append(f"markers<{args.min_markers}")
+            if corners < args.min_charuco_corners:
+                reasons.append(f"charuco<{args.min_charuco_corners}")
+            record["reason"] = ",".join(reasons) if reasons else "rejected"
+            rejected.append(record)
+
+    (output_dir / "accepted_images.json").write_text(json.dumps(accepted, indent=2), encoding="utf-8")
+    (output_dir / "rejected_images.json").write_text(json.dumps(rejected, indent=2), encoding="utf-8")
+
+    summary = {
+        "assets_dir": str(assets_dir.resolve()),
+        "dictionary": args.dictionary,
+        "squares_x": args.squares_x,
+        "squares_y": args.squares_y,
+        "square_length_mm": args.square_length_mm,
+        "marker_length_mm": args.marker_length_mm,
+        "min_markers": args.min_markers,
+        "min_charuco_corners": args.min_charuco_corners,
+        "total_images": len(files),
+        "accepted_images": len(accepted),
+        "rejected_images": len(rejected),
+    }
+
+    print(f"Images found: {len(files)}")
+    print(f"Accepted: {len(accepted)}")
+    print(f"Rejected: {len(rejected)}")
+
+    if len(accepted) < args.min_images:
+        summary["status"] = "not_enough_images"
+        (output_dir / "calibration_report.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
+        raise SystemExit(f"Not enough accepted images for calibration: {len(accepted)} < {args.min_images}")
+
+    retval, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.aruco.calibrateCameraCharuco(
+        charucoCorners=all_charuco_corners,
+        charucoIds=all_charuco_ids,
+        board=board,
+        imageSize=image_size,
+        cameraMatrix=None,
+        distCoeffs=None,
+    )
+
+    np.savez(
+        output_dir / "camera_calibration.npz",
+        camera_matrix=camera_matrix,
+        dist_coeffs=dist_coeffs,
+        image_width=image_size[0],
+        image_height=image_size[1],
+        dictionary=args.dictionary,
+        squares_x=args.squares_x,
+        squares_y=args.squares_y,
+        square_length_mm=args.square_length_mm,
+        marker_length_mm=args.marker_length_mm,
+    )
+    write_yaml(output_dir / "camera_calibration.yaml", camera_matrix, dist_coeffs)
+
+    summary.update(
+        {
+            "status": "ok",
+            "image_width": image_size[0],
+            "image_height": image_size[1],
+            "rms_reprojection_error": float(retval),
+            "camera_matrix": camera_matrix.tolist(),
+            "dist_coeffs": dist_coeffs.tolist(),
+        }
+    )
+    (output_dir / "calibration_report.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
+
+    print(f"Calibration RMS reprojection error: {retval:.6f}")
+    print(f"Report written to: {output_dir / 'calibration_report.json'}")
+    print(f"NPZ written to:    {output_dir / 'camera_calibration.npz'}")
+    print(f"YAML written to:   {output_dir / 'camera_calibration.yaml'}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

calibration_output/accepted_images.json

@@ -0,0 +1,191 @@
+[
+  {
+    "file": "IMG_20260606_093157.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093204.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093212.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093219.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093237.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093257.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093305.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093421.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093611.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093615.jpg",
+    "markers": 13,
+    "charuco_corners": 15,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093627.jpg",
+    "markers": 16,
+    "charuco_corners": 22,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093646.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093656.jpg",
+    "markers": 13,
+    "charuco_corners": 13,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093714.jpg",
+    "markers": 15,
+    "charuco_corners": 18,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093751.jpg",
+    "markers": 16,
+    "charuco_corners": 20,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093815.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_093935.jpg",
+    "markers": 16,
+    "charuco_corners": 20,
+    "width": 4000,
+    "height": 3000
+  },
+  {
+    "file": "IMG_20260606_093944.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 4000,
+    "height": 3000
+  },
+  {
+    "file": "IMG_20260606_093948.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_094016.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_094028.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_094032.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_094037.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 3000,
+    "height": 4000
+  },
+  {
+    "file": "IMG_20260606_094042.jpg",
+    "markers": 16,
+    "charuco_corners": 20,
+    "width": 4000,
+    "height": 3000
+  },
+  {
+    "file": "IMG_20260606_094047.jpg",
+    "markers": 16,
+    "charuco_corners": 22,
+    "width": 4000,
+    "height": 3000
+  },
+  {
+    "file": "IMG_20260606_094052.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 4000,
+    "height": 3000
+  },
+  {
+    "file": "IMG_20260606_094057.jpg",
+    "markers": 17,
+    "charuco_corners": 24,
+    "width": 4000,
+    "height": 3000
+  }
+]

calibration_output/calibration_report.json

@@ -0,0 +1,43 @@
+{
+  "assets_dir": "C:\\devel\\slam_demo\\assets",
+  "dictionary": "DICT_4X4_50",
+  "squares_x": 7,
+  "squares_y": 5,
+  "square_length_mm": 25.0,
+  "marker_length_mm": 18.75,
+  "min_markers": 8,
+  "min_charuco_corners": 12,
+  "total_images": 43,
+  "accepted_images": 27,
+  "rejected_images": 16,
+  "status": "ok",
+  "image_width": 3000,
+  "image_height": 4000,
+  "rms_reprojection_error": 0.4331525306786906,
+  "camera_matrix": [
+    [
+      2922.5451296678484,
+      0.0,
+      1513.228003099643
+    ],
+    [
+      0.0,
+      2929.578022490368,
+      1987.1398774988904
+    ],
+    [
+      0.0,
+      0.0,
+      1.0
+    ]
+  ],
+  "dist_coeffs": [
+    [
+      0.06569042692273651,
+      -0.12285829543468728,
+      -0.0017248793581868354,
+      -0.00019726508785207679,
+      0.07981683482477274
+    ]
+  ]
+}

calibration_output/camera_calibration.yaml

@@ -0,0 +1,15 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 2922.5451296678484, 0., 1513.228003099643, 0.,
+       2929.5780224903679, 1987.1398774988904, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 0.065690426922736508, -0.12285829543468728,
+       -0.0017248793581868354, -0.00019726508785207679,
+       0.079816834824772739 ]

calibration_output/rejected_images.json

@@ -0,0 +1,130 @@
+[
+  {
+    "file": "IMG_20260606_093320.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093434.jpg",
+    "markers": 1,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093444.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093455.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093505.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093515.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093525.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093726.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093738.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093758.jpg",
+    "markers": 28,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093804.jpg",
+    "markers": 36,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093825.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093835.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093902.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093939.jpg",
+    "markers": 5,
+    "charuco_corners": 3,
+    "width": 4000,
+    "height": 3000,
+    "reason": "markers<8,charuco<12"
+  },
+  {
+    "file": "IMG_20260606_093959.jpg",
+    "markers": 0,
+    "charuco_corners": 0,
+    "width": 3000,
+    "height": 4000,
+    "reason": "markers<8,charuco<12"
+  }
+]

generate_apriltag_set.py

@@ -0,0 +1,107 @@
+from pathlib import Path
+
+import cv2
+import numpy as np
+from PIL import Image, ImageDraw, ImageFont
+
+A4_WIDTH_PX = 2480
+A4_HEIGHT_PX = 3508
+DPI = 300
+TAG_SIDE_MM = 200.0
+TAG_SIDE_PX = round(TAG_SIDE_MM / 25.4 * DPI)
+DICT_NAME = "DICT_APRILTAG_36h11"
+DICT_ID = cv2.aruco.DICT_APRILTAG_36h11
+
+TAGS = [
+    (1, "base_home", "BASE_HOME", "Tag base drone / home position"),
+    (10, "fronte_scaffali_centrali", "FRONTE_SCAFFALI", "Fronte 2 scaffali centrali"),
+    (21, "inizio_scaffale_basso", "START_LOW", "Inizio scaffale quota 1.0 m"),
+    (22, "inizio_scaffale_medio", "START_MID", "Inizio scaffale quota 2.0 m"),
+    (23, "inizio_scaffale_alto", "START_HIGH", "Inizio scaffale quota 3.0 m"),
+    (31, "fine_scaffale_basso", "END_LOW", "Fine scaffale quota 1.0 m"),
+    (32, "fine_scaffale_medio", "END_MID", "Fine scaffale quota 2.0 m"),
+    (33, "fine_scaffale_alto", "END_HIGH", "Fine scaffale quota 3.0 m"),
+]
+
+OUT_DIR = Path(r"C:\devel\slam_demo\tags_set_a4_20cm")
+OUT_DIR.mkdir(parents=True, exist_ok=True)
+
+aruco_dict = cv2.aruco.getPredefinedDictionary(DICT_ID)
+
+try:
+    title_font = ImageFont.truetype("arial.ttf", 72)
+    meta_font = ImageFont.truetype("arial.ttf", 46)
+    small_font = ImageFont.truetype("arial.ttf", 34)
+except OSError:
+    title_font = ImageFont.load_default()
+    meta_font = ImageFont.load_default()
+    small_font = ImageFont.load_default()
+
+info_lines = [
+    f"Dictionary: {DICT_NAME}",
+    f"A4 page: {A4_WIDTH_PX}x{A4_HEIGHT_PX}px @ {DPI} dpi",
+    f"Marker side target: {TAG_SIDE_MM:.1f} mm",
+    "Print settings: 100% scale, no fit-to-page, no auto-resize.",
+    "Verification after print: measure the black square side, it must be 200 mm.",
+    "",
+    "Tag set and placement:",
+]
+
+contact_sheet = Image.new("RGB", (A4_WIDTH_PX * 2, A4_HEIGHT_PX * 4), "white")
+
+for index, (tag_id, slug, short_name, meaning) in enumerate(TAGS):
+    marker = cv2.aruco.generateImageMarker(aruco_dict, tag_id, TAG_SIDE_PX)
+    marker_rgb = cv2.cvtColor(marker, cv2.COLOR_GRAY2RGB)
+    marker_img = Image.fromarray(marker_rgb)
+
+    page = Image.new("RGB", (A4_WIDTH_PX, A4_HEIGHT_PX), "white")
+    draw = ImageDraw.Draw(page)
+
+    title = f"APRILTAG {tag_id:02d}"
+    subtitle = short_name
+    footer = meaning
+    verify = "Verifica stampa: lato quadrato nero = 200 mm"
+
+    title_bbox = draw.textbbox((0, 0), title, font=title_font)
+    subtitle_bbox = draw.textbbox((0, 0), subtitle, font=meta_font)
+    footer_bbox = draw.textbbox((0, 0), footer, font=small_font)
+    verify_bbox = draw.textbbox((0, 0), verify, font=small_font)
+
+    top_y = 120
+    draw.text(((A4_WIDTH_PX - (title_bbox[2] - title_bbox[0])) / 2, top_y), title, fill="black", font=title_font)
+    draw.text(((A4_WIDTH_PX - (subtitle_bbox[2] - subtitle_bbox[0])) / 2, top_y + 90), subtitle, fill="black", font=meta_font)
+
+    marker_x = (A4_WIDTH_PX - TAG_SIDE_PX) // 2
+    marker_y = 360
+    page.paste(marker_img, (marker_x, marker_y))
+
+    footer_y = marker_y + TAG_SIDE_PX + 80
+    draw.text(((A4_WIDTH_PX - (footer_bbox[2] - footer_bbox[0])) / 2, footer_y), footer, fill="black", font=small_font)
+    draw.text(((A4_WIDTH_PX - (verify_bbox[2] - verify_bbox[0])) / 2, footer_y + 60), verify, fill="black", font=small_font)
+
+    png_path = OUT_DIR / f"apriltag_{tag_id:02d}_{slug}_a4_20cm.png"
+    page.save(png_path, dpi=(DPI, DPI))
+
+    info_lines.append(f"- ID {tag_id:02d} | {short_name} | {meaning}")
+
+    thumb = page.copy()
+    thumb.thumbnail((A4_WIDTH_PX, A4_HEIGHT_PX))
+    row = index // 2
+    col = index % 2
+    contact_sheet.paste(thumb, (col * A4_WIDTH_PX, row * A4_HEIGHT_PX))
+
+info_lines.extend([
+    "",
+    "Placement instructions:",
+    "- ID 01 BASE_HOME: attach near the drone base/home position, clearly visible from the parking/start pose.",
+    "- ID 10 FRONTE_SCAFFALI: attach on the wall or fixed support in front of the two central shelves, at approx. 2.0 m height.",
+    "- IDs 21/22/23 START_*: attach vertically on the start side testata scaffale at 1.0 m, 2.0 m, 3.0 m.",
+    "- IDs 31/32/33 END_*: attach vertically on the end side testata scaffale at 1.0 m, 2.0 m, 3.0 m.",
+    "- Keep each tag planar, rigid, and orthogonal to the expected viewing direction.",
+    "- Avoid reflections, wrinkles, and partial occlusions.",
+    "- Keep at least 10-15 cm vertical separation between printed sheets when mounting the stacked tags.",
+])
+
+(OUT_DIR / "apriltag_semantics_and_placement.txt").write_text("\n".join(info_lines), encoding="utf-8")
+contact_sheet.save(OUT_DIR / "apriltag_contact_sheet.png", dpi=(DPI, DPI))
+print(f"Generated {len(TAGS)} tags in {OUT_DIR}")

generate_apriltag_set_pdf.py

@@ -0,0 +1,131 @@
+from pathlib import Path
+
+import cv2
+from PIL import Image, ImageDraw, ImageFont
+
+
+A4_WIDTH_PX = 2480
+A4_HEIGHT_PX = 3508
+DPI = 300
+TAG_SIDE_MM = 200.0
+TAG_SIDE_PX = round(TAG_SIDE_MM / 25.4 * DPI)
+DICT_NAME = "DICT_APRILTAG_36h11"
+DICT_ID = cv2.aruco.DICT_APRILTAG_36h11
+
+TAGS = [
+    (1, "base_home", "BASE_HOME", "Tag base drone / home position"),
+    (10, "fronte_scaffali_centrali", "FRONTE_SCAFFALI", "Fronte 2 scaffali centrali"),
+    (21, "inizio_scaffale_basso", "START_LOW", "Inizio scaffale quota 1.0 m"),
+    (22, "inizio_scaffale_medio", "START_MID", "Inizio scaffale quota 2.0 m"),
+    (23, "inizio_scaffale_alto", "START_HIGH", "Inizio scaffale quota 3.0 m"),
+    (31, "fine_scaffale_basso", "END_LOW", "Fine scaffale quota 1.0 m"),
+    (32, "fine_scaffale_medio", "END_MID", "Fine scaffale quota 2.0 m"),
+    (33, "fine_scaffale_alto", "END_HIGH", "Fine scaffale quota 3.0 m"),
+]
+
+OUT_DIR = Path(r"C:\devel\slam_demo\tags_set_a4_20cm_pdf")
+OUT_DIR.mkdir(parents=True, exist_ok=True)
+
+
+def mm_to_px(mm: float) -> int:
+    return round(mm / 25.4 * DPI)
+
+
+def load_font(name: str, size: int):
+    try:
+        return ImageFont.truetype(name, size)
+    except OSError:
+        return ImageFont.load_default()
+
+
+def draw_centered(draw: ImageDraw.ImageDraw, y: int, text: str, font, fill="black") -> None:
+    bbox = draw.textbbox((0, 0), text, font=font)
+    w = bbox[2] - bbox[0]
+    x = (A4_WIDTH_PX - w) // 2
+    draw.text((x, y), text, fill=fill, font=font)
+
+
+def draw_measure_line(draw: ImageDraw.ImageDraw, x: int, y: int, width_px: int, label: str, font) -> None:
+    tick = 20
+    draw.line((x, y, x + width_px, y), fill="black", width=3)
+    draw.line((x, y - tick, x, y + tick), fill="black", width=3)
+    draw.line((x + width_px, y - tick, x + width_px, y + tick), fill="black", width=3)
+    bbox = draw.textbbox((0, 0), label, font=font)
+    text_w = bbox[2] - bbox[0]
+    draw.text((x + (width_px - text_w) // 2, y - 60), label, fill="black", font=font)
+
+
+def generate():
+    aruco_dict = cv2.aruco.getPredefinedDictionary(DICT_ID)
+
+    title_font = load_font("arial.ttf", 72)
+    meta_font = load_font("arial.ttf", 46)
+    small_font = load_font("arial.ttf", 34)
+
+    info_lines = [
+        f"Dictionary: {DICT_NAME}",
+        f"A4 page: {A4_WIDTH_PX}x{A4_HEIGHT_PX}px @ {DPI} dpi",
+        f"Marker side target: {TAG_SIDE_MM:.1f} mm",
+        "Print settings: actual size / 100%, no fit-to-page, no shrink, no borderless auto-scale.",
+        "Verification after print: black square side must be 200 mm. Secondary ruler line must be 100 mm.",
+        "",
+        "Tag set and placement:",
+    ]
+
+    pages = []
+
+    for tag_id, slug, short_name, meaning in TAGS:
+        marker = cv2.aruco.generateImageMarker(aruco_dict, tag_id, TAG_SIDE_PX)
+        marker_img = Image.fromarray(marker).convert("RGB")
+
+        page = Image.new("RGB", (A4_WIDTH_PX, A4_HEIGHT_PX), "white")
+        draw = ImageDraw.Draw(page)
+
+        draw_centered(draw, 120, f"APRILTAG {tag_id:02d}", title_font)
+        draw_centered(draw, 210, short_name, meta_font)
+
+        marker_x = (A4_WIDTH_PX - TAG_SIDE_PX) // 2
+        marker_y = 360
+        page.paste(marker_img, (marker_x, marker_y))
+
+        draw_centered(draw, marker_y + TAG_SIDE_PX + 70, meaning, small_font)
+        draw_centered(draw, marker_y + TAG_SIDE_PX + 120, "Verifica stampa: lato quadrato nero = 200 mm", small_font)
+
+        line_width = mm_to_px(100.0)
+        line_x = (A4_WIDTH_PX - line_width) // 2
+        line_y = marker_y + TAG_SIDE_PX + 230
+        draw_measure_line(draw, line_x, line_y, line_width, "Linea campione = 100 mm", small_font)
+
+        pdf_path = OUT_DIR / f"apriltag_{tag_id:02d}_{slug}_a4_20cm.pdf"
+        png_path = OUT_DIR / f"apriltag_{tag_id:02d}_{slug}_a4_20cm_preview.png"
+        page.save(pdf_path, "PDF", resolution=DPI)
+        page.save(png_path, dpi=(DPI, DPI))
+        pages.append(page.copy())
+
+        info_lines.append(f"- ID {tag_id:02d} | {short_name} | {meaning}")
+
+    info_lines.extend(
+        [
+            "",
+            "Placement instructions:",
+            "- ID 01 BASE_HOME: attach near the drone base/home position, clearly visible from the parking/start pose.",
+            "- ID 10 FRONTE_SCAFFALI: attach on the wall or fixed support in front of the two central shelves, at approx. 2.0 m height.",
+            "- IDs 21/22/23 START_*: attach vertically on the start side testata scaffale at 1.0 m, 2.0 m, 3.0 m.",
+            "- IDs 31/32/33 END_*: attach vertically on the end side testata scaffale at 1.0 m, 2.0 m, 3.0 m.",
+            "- Keep each tag planar, rigid, and orthogonal to the expected viewing direction.",
+            "- Avoid reflections, wrinkles, and partial occlusions.",
+            "- If measured size is wrong, do not compensate in software: fix the printer scaling first.",
+        ]
+    )
+    (OUT_DIR / "apriltag_print_and_placement_instructions.txt").write_text(
+        "\n".join(info_lines), encoding="utf-8"
+    )
+
+    if pages:
+        bundle_path = OUT_DIR / "apriltag_bundle_a4_20cm.pdf"
+        first, rest = pages[0], pages[1:]
+        first.save(bundle_path, "PDF", resolution=DPI, save_all=True, append_images=rest)
+
+
+if __name__ == "__main__":
+    generate()

tags/apriltag_info.txt

@@ -0,0 +1,7 @@
+AprilTag dictionary: DICT_APRILTAG_36h11
+Print note: print at 100% scale, no fit-to-page, no auto-resize
+Suggested print target: A4, centered, with white margins preserved
+
+ID 10 -> POS_FRONTE_LATERALE_SCAFFALI
+ID 21 -> START_SCAN_L2M_LR
+          meaning: start scan at 2.0 m height, left to right

tags_a4_20cm/apriltag_print_info.txt

@@ -0,0 +1,8 @@
+AprilTag dictionary: DICT_APRILTAG_36h11
+Target print size: marker side = 200 mm
+Paper: A4 portrait
+Print at 100% scale, no fit-to-page, no auto-resize
+After printing, measure the black square side: it must be 200 mm
+
+ID 10 -> POS_FRONTE_LATERALE_SCAFFALI
+ID 21 -> START_SCAN_L2M_LR

tags_set_a4_20cm/apriltag_semantics_and_placement.txt

@@ -0,0 +1,24 @@
+Dictionary: DICT_APRILTAG_36h11
+A4 page: 2480x3508px @ 300 dpi
+Marker side target: 200.0 mm
+Print settings: 100% scale, no fit-to-page, no auto-resize.
+Verification after print: measure the black square side, it must be 200 mm.
+
+Tag set and placement:
+- ID 01 | BASE_HOME | Tag base drone / home position
+- ID 10 | FRONTE_SCAFFALI | Fronte 2 scaffali centrali
+- ID 21 | START_LOW | Inizio scaffale quota 1.0 m
+- ID 22 | START_MID | Inizio scaffale quota 2.0 m
+- ID 23 | START_HIGH | Inizio scaffale quota 3.0 m
+- ID 31 | END_LOW | Fine scaffale quota 1.0 m
+- ID 32 | END_MID | Fine scaffale quota 2.0 m
+- ID 33 | END_HIGH | Fine scaffale quota 3.0 m
+
+Placement instructions:
+- ID 01 BASE_HOME: attach near the drone base/home position, clearly visible from the parking/start pose.
+- ID 10 FRONTE_SCAFFALI: attach on the wall or fixed support in front of the two central shelves, at approx. 2.0 m height.
+- IDs 21/22/23 START_*: attach vertically on the start side testata scaffale at 1.0 m, 2.0 m, 3.0 m.
+- IDs 31/32/33 END_*: attach vertically on the end side testata scaffale at 1.0 m, 2.0 m, 3.0 m.
+- Keep each tag planar, rigid, and orthogonal to the expected viewing direction.
+- Avoid reflections, wrinkles, and partial occlusions.
+- Keep at least 10-15 cm vertical separation between printed sheets when mounting the stacked tags.

tags_set_a4_20cm_pdf/apriltag_print_and_placement_instructions.txt

@@ -0,0 +1,24 @@
+Dictionary: DICT_APRILTAG_36h11
+A4 page: 2480x3508px @ 300 dpi
+Marker side target: 200.0 mm
+Print settings: actual size / 100%, no fit-to-page, no shrink, no borderless auto-scale.
+Verification after print: black square side must be 200 mm. Secondary ruler line must be 100 mm.
+
+Tag set and placement:
+- ID 01 | BASE_HOME | Tag base drone / home position
+- ID 10 | FRONTE_SCAFFALI | Fronte 2 scaffali centrali
+- ID 21 | START_LOW | Inizio scaffale quota 1.0 m
+- ID 22 | START_MID | Inizio scaffale quota 2.0 m
+- ID 23 | START_HIGH | Inizio scaffale quota 3.0 m
+- ID 31 | END_LOW | Fine scaffale quota 1.0 m
+- ID 32 | END_MID | Fine scaffale quota 2.0 m
+- ID 33 | END_HIGH | Fine scaffale quota 3.0 m
+
+Placement instructions:
+- ID 01 BASE_HOME: attach near the drone base/home position, clearly visible from the parking/start pose.
+- ID 10 FRONTE_SCAFFALI: attach on the wall or fixed support in front of the two central shelves, at approx. 2.0 m height.
+- IDs 21/22/23 START_*: attach vertically on the start side testata scaffale at 1.0 m, 2.0 m, 3.0 m.
+- IDs 31/32/33 END_*: attach vertically on the end side testata scaffale at 1.0 m, 2.0 m, 3.0 m.
+- Keep each tag planar, rigid, and orthogonal to the expected viewing direction.
+- Avoid reflections, wrinkles, and partial occlusions.
+- If measured size is wrong, do not compensate in software: fix the printer scaling first.