95 lines
3 KiB
Python
95 lines
3 KiB
Python
"""
|
|
calibration.py — Robot Tracker v007
|
|
Berechnet planare Homographie aus 4 Referenz-AprilTags.
|
|
Wandelt Kamera-Pixel in Weltkoordinaten (mm) um.
|
|
"""
|
|
|
|
import logging
|
|
import numpy as np
|
|
import cv2
|
|
from typing import Optional
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
class Calibration:
|
|
def __init__(self, reference_tags: dict):
|
|
"""
|
|
reference_tags: {tag_id: [x_mm, y_mm], ...}
|
|
Beispiel: {0: [0, 0], 1: [100.5, 0], 2: [0, 181], 3: [100.5, 181]}
|
|
"""
|
|
self.reference_tags = reference_tags
|
|
self.H: Optional[np.ndarray] = None # Homographie-Matrix Pixel → mm
|
|
self.H_inv: Optional[np.ndarray] = None # mm → Pixel
|
|
self.calibrated = False
|
|
|
|
def update(self, detected_tags: list) -> bool:
|
|
"""
|
|
Versucht Homographie aus aktuell sichtbaren Referenz-Tags zu berechnen.
|
|
Gibt True zurück wenn Kalibrierung erfolgreich.
|
|
"""
|
|
# Nur Referenz-Tags filtern
|
|
ref_detected = {
|
|
t["id"]: t
|
|
for t in detected_tags
|
|
if t["id"] in self.reference_tags
|
|
}
|
|
|
|
if len(ref_detected) < 4:
|
|
return False
|
|
|
|
# Punkte aufbauen
|
|
pts_pixel = []
|
|
pts_world = []
|
|
|
|
for tag_id, world_pos in self.reference_tags.items():
|
|
if tag_id in ref_detected:
|
|
tag = ref_detected[tag_id]
|
|
pts_pixel.append([tag["x"], tag["y"]])
|
|
pts_world.append(world_pos)
|
|
|
|
if len(pts_pixel) < 4:
|
|
return False
|
|
|
|
src = np.array(pts_pixel, dtype=np.float32)
|
|
dst = np.array(pts_world, dtype=np.float32)
|
|
|
|
H, mask = cv2.findHomography(src, dst, cv2.RANSAC, 5.0)
|
|
|
|
if H is None:
|
|
logger.warning("Homographie-Berechnung fehlgeschlagen")
|
|
return False
|
|
|
|
self.H = H
|
|
self.H_inv = np.linalg.inv(H)
|
|
self.calibrated = True
|
|
logger.info("Kalibrierung erfolgreich!")
|
|
return True
|
|
|
|
def pixel_to_world(self, x_px: float, y_px: float) -> tuple[float, float]:
|
|
"""Pixel → Weltkoordinaten (mm)"""
|
|
if not self.calibrated:
|
|
return x_px, y_px
|
|
pt = np.array([[[x_px, y_px]]], dtype=np.float32)
|
|
result = cv2.perspectiveTransform(pt, self.H)
|
|
return float(result[0][0][0]), float(result[0][0][1])
|
|
|
|
def world_to_pixel(self, x_mm: float, y_mm: float) -> tuple[float, float]:
|
|
"""Weltkoordinaten (mm) → Pixel"""
|
|
if not self.calibrated:
|
|
return x_mm, y_mm
|
|
pt = np.array([[[x_mm, y_mm]]], dtype=np.float32)
|
|
result = cv2.perspectiveTransform(pt, self.H_inv)
|
|
return float(result[0][0][0]), float(result[0][0][1])
|
|
|
|
def get_field_corners_px(self) -> Optional[list]:
|
|
"""Gibt die Spielfeldecken in Pixel zurück (für Visualisierung)"""
|
|
if not self.calibrated:
|
|
return None
|
|
corners_world = [
|
|
[0, 0],
|
|
[100.5, 0],
|
|
[100.5, 181],
|
|
[0, 181],
|
|
]
|
|
return [self.world_to_pixel(x, y) for x, y in corners_world]
|