diff --git a/scripts/calibrazione.py b/scripts/calibrazione.py index 76abd81..3d1d8a8 100644 --- a/scripts/calibrazione.py +++ b/scripts/calibrazione.py @@ -3,10 +3,16 @@ Cosa fa: da una foto di scacchiera piana calcola la scala (px/mm) con TieVision-Core (visionsuite.calibration), e conserva un risultato per sorgente camera in un JSON (scrittura atomica). +#84: `calibra_scacchiera_densa(pixels, square_mm)` calibra da UNA foto di +scacchiera che riempie tutto il campo (ottica telecentrica): trova da sola la +griglia, costruisce la mappa di distorsione (DistortionMap) e il mm/px +dell'immagine raddrizzata. L'Archivio salva la mappa accanto al JSON (.npz) e +`raddrizza()` la applica ai frame. Uso: importato da scripts/camera_service.py (rotte /camera/calibrate e -/camera/calibration); `calibra_scacchiera(gray, cols, rows, square_mm)` e' -logica pura, senza FastAPI, e si prova da sola. -Dipende da: numpy, opencv, visionsuite (vs-core). Niente altro. +/camera/calibration); `calibra_scacchiera(gray, cols, rows, square_mm)` e +`calibra_scacchiera_densa(gray, square_mm)` sono logica pura, senza FastAPI, +e si provano da sole. +Dipende da: numpy, opencv, visionsuite (vs-core). Niente altro (no scipy). """ from __future__ import annotations @@ -17,13 +23,17 @@ import math import os import re import tempfile +import threading +from collections import deque from datetime import datetime, timezone from pathlib import Path +from typing import NamedTuple import cv2 import numpy as np from visionsuite.calibration import calibrate_planar, find_chessboard +from visionsuite.calibration.distortion_map import DistortionMap # #84 mappa densa from visionsuite.core.image import Image @@ -47,7 +57,8 @@ def calibra_scacchiera(pixels, cols: int, rows: int, square_mm: float) -> dict: """Scala (px/mm) da una foto di scacchiera piana. cols/rows = angoli INTERNI. Ritorna {px_per_mm, mm_per_px, rms_px, n_corners, cols, rows, square_mm, - image_width, image_height, scale_spread_pct, overlay_png (b64)}. + image_width, image_height, scale_spread_pct, metodo, distorsione, + overlay_png (b64)}. """ if cols < 2 or rows < 2: raise CalibrazioneError("cols e rows (angoli interni) devono essere almeno 2") @@ -95,19 +106,357 @@ def calibra_scacchiera(pixels, cols: int, rows: int, square_mm: float) -> dict: "image_width": int(w), "image_height": int(h), "scale_spread_pct": float(spread), + "metodo": "planare", # #84: il metodo con cols/rows resta quello di prima + "distorsione": False, "overlay_png": base64.b64encode(png.tobytes()).decode("ascii"), } +# ---- #84: calibrazione densa (scacchiera che riempie il campo, griglia non nota) + +# #84: findChessboardCornersSBWithMeta (verificato su opencv 5.0.0) su una scacchiera +# grande quanto l'immagine trova solo frammenti (2064x1544 con quadrati da 40 px: +# 5x5, 28x6...). Per questo la griglia si cerca qui: sellini della Hessiana a piu' +# scale + crescita del reticolo; poi cornerSubPix. +_SIGME = (1.5, 2.2, 3.2, 4.5, 6.5, 9.0, 13.0) # scale dei sellini (passo ~ 5..120 px) +_MAX_CANDIDATI = 6000 + + +def _candidati(gray: np.ndarray, sigma: float) -> np.ndarray: + """Massimi locali del 'selliness' (-det Hessiana) = angoli di scacchiera, (n,2) x,y.""" + b = cv2.GaussianBlur(gray.astype(np.float32), (0, 0), sigma) + ixx = cv2.Sobel(b, cv2.CV_32F, 2, 0, ksize=3) + iyy = cv2.Sobel(b, cv2.CV_32F, 0, 2, ksize=3) + ixy = cv2.Sobel(b, cv2.CV_32F, 1, 1, ksize=3) + r = ixy * ixy - ixx * iyy # >0 solo dove l'immagine ha un sellino + massimo = float(r.max()) + if massimo <= 0: + return np.zeros((0, 2)) + k = 2 * int(math.ceil(1.5 * sigma)) + 1 + picchi = (r >= cv2.dilate(r, np.ones((k, k), np.uint8))) & (r > 0.1 * massimo) + ys, xs = np.nonzero(picchi) + if len(xs) > _MAX_CANDIDATI: # tiene i piu' forti + forti = np.argsort(r[ys, xs])[::-1][:_MAX_CANDIDATI] + ys, xs = ys[forti], xs[forti] + return np.stack([xs, ys], axis=1).astype(np.float64) + + +def _e_angolo_x(lisc: np.ndarray, delta: float, p: np.ndarray, u: np.ndarray, v: np.ndarray) -> bool: + """True se attorno a p i quattro quadranti alternano chiaro/scuro (vero angolo X). + + Scarta le giunzioni a L/T al bordo della scacchiera (3 quadranti uguali), che + il selliness da solo accetta e che spostano la griglia. + """ + h, w = lisc.shape + campioni = [] + for q in (0.25 * (u + v), -0.25 * (u + v), 0.25 * (u - v), -0.25 * (u - v)): + x, y = int(round(p[0] + q[0])), int(round(p[1] + q[1])) + if not (0 <= x < w and 0 <= y < h): + return False + campioni.append(float(lisc[y, x])) + s1, s2, t1, t2 = campioni + return (min(s1, s2) > max(t1, t2) + delta) or (max(s1, s2) < min(t1, t2) - delta) + + +def _cresci_reticolo(pts: np.ndarray, centro: tuple[float, float], + lisc: np.ndarray, delta: float) -> dict | None: + """Assegna un indice (riga, colonna) ai candidati che stanno su un reticolo. + + Parte dal candidato piu' vicino al centro, ricava i due vettori di passo dai + vicini e cresce a macchia d'olio: la base si aggiorna a ogni passo, quindi + segue una distorsione lenta. Ritorna {(i, j): indice candidato} o None. + """ + if len(pts) < 9: + return None + seme = int(np.argmin(np.hypot(pts[:, 0] - centro[0], pts[:, 1] - centro[1]))) + d = np.hypot(pts[:, 0] - pts[seme, 0], pts[:, 1] - pts[seme, 1]) + vicini = np.argsort(d)[1:9] + u = pts[vicini[0]] - pts[seme] + lu = float(np.hypot(*u)) + v = None + for k in vicini[1:]: + w = pts[k] - pts[seme] + lw = float(np.hypot(*w)) + # il secondo vettore: quasi ortogonale al primo e di lunghezza simile + if lu > 0 and lw > 0 and abs(float(u @ w)) / (lu * lw) < 0.4 and lw < 1.5 * lu: + v = w + break + if v is None or lu < 3: + return None + posto = {(0, 0): seme} + indice = {seme: (0, 0)} + basi = {seme: (u, v)} + coda = deque([seme]) + while coda: + a = coda.popleft() + i, j = indice[a] + u, v = basi[a] + tol = 0.3 * min(float(np.hypot(*u)), float(np.hypot(*v))) + for di, dj, vec in ((0, 1, u), (0, -1, -u), (1, 0, v), (-1, 0, -v)): + if (i + di, j + dj) in posto: + continue + prev = pts[a] + vec + dist = np.hypot(pts[:, 0] - prev[0], pts[:, 1] - prev[1]) + b = int(np.argmin(dist)) + if dist[b] > tol or b in indice: + continue + if not _e_angolo_x(lisc, delta, pts[b], u, v): # niente L/T del bordo + continue + posto[(i + di, j + dj)] = b + indice[b] = (i + di, j + dj) + reale = pts[b] - pts[a] + basi[b] = (reale * dj, v) if dj else (u, reale * di) # base locale aggiornata + coda.append(b) + return posto + + +def _rettangolo_pieno(posto: dict) -> tuple[int, int, int, int] | None: + """Il rettangolo di indici tutto occupato di area massima: (i0, j0, righe, colonne).""" + if not posto: + return None + i_min = min(i for i, _ in posto) + j_min = min(j for _, j in posto) + nr = max(i for i, _ in posto) - i_min + 1 + nc = max(j for _, j in posto) - j_min + 1 + m = np.zeros((nr, nc), bool) + for (i, j) in posto: + m[i - i_min, j - j_min] = True + migliore = (0, 0, 0, 0) + alt = np.zeros(nc, int) + for r in range(nr): # istogramma delle altezze + pila, O(righe*colonne) + alt = np.where(m[r], alt + 1, 0) + pila: list[int] = [] + for c in range(nc + 1): + h = int(alt[c]) if c < nc else 0 + while pila and alt[pila[-1]] >= h: + altezza = int(alt[pila.pop()]) + sx = pila[-1] + 1 if pila else 0 + larg = c - sx + if altezza * larg > migliore[2] * migliore[3]: + migliore = (r - altezza + 1 + i_min, sx + j_min, altezza, larg) + pila.append(c) + return migliore if migliore[2] * migliore[3] else None + + +def _orienta(g: np.ndarray) -> np.ndarray: + """Riordina (nr,nc,2) con colonne verso destra e righe verso il basso.""" + a = (g[:, -1] - g[:, 0]).mean(axis=0) # direzione dell'indice di colonna + b = (g[-1, :] - g[0, :]).mean(axis=0) # direzione dell'indice di riga + if abs(b[0]) > abs(a[0]): # assi scambiati (scacchiera ruotata di ~90 gradi) + g = g.transpose(1, 0, 2) + a, b = b, a + if a[0] < 0: + g = g[:, ::-1] + if b[1] < 0: + g = g[::-1] + return np.ascontiguousarray(g) + + +def _passo_mediano(g: np.ndarray) -> float: + """Distanza mediana (px) fra corner adiacenti.""" + passi = np.concatenate([np.diff(g, axis=1).reshape(-1, 2), np.diff(g, axis=0).reshape(-1, 2)]) + return float(np.median(np.hypot(passi[:, 0], passi[:, 1]))) + + +def _trova_griglia(frame: np.ndarray) -> np.ndarray: + """Griglia di angoli (n_righe, n_colonne, 2) x,y, in ordine di riga dall'alto.""" + h, w = frame.shape + migliore = None + lisc = cv2.GaussianBlur(frame.astype(np.float32), (0, 0), 1.0) + p5, p95 = np.percentile(frame, (5, 95)) + delta = 0.25 * float(p95 - p5) # contrasto minimo fra quadranti opposti + for sigma in _SIGME: + pts = _candidati(frame, sigma) + posto = _cresci_reticolo(pts, (w / 2.0, h / 2.0), lisc, delta) + rett = _rettangolo_pieno(posto) if posto else None + if rett is None: + continue + i0, j0, nr, nc = rett + if nr < 3 or nc < 3: + continue + if migliore is not None and nr * nc < 0.5 * migliore[0] * migliore[1]: + break # oltre il picco: le scale piu' grosse fondono gli angoli + if migliore is None or nr * nc > migliore[0] * migliore[1]: + g = np.array([[pts[posto[(i0 + i, j0 + j)]] for j in range(nc)] + for i in range(nr)]) + migliore = (nr, nc, g) + if migliore is None: + raise CalibrazioneError( + "griglia della scacchiera non trovata: serve una scacchiera nitida " + "che riempia il campo") + g = _orienta(migliore[2]) + nr, nc, _ = g.shape + passo = _passo_mediano(g) + # sub-pixel: finestra proporzionale al passo, abbastanza piccola da non vedere i vicini + win = int(min(20, max(3, 0.3 * passo))) + crit = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 40, 0.001) + grezzi = g.reshape(-1, 1, 2).astype(np.float32) + raff = cv2.cornerSubPix(frame, grezzi.copy(), (win, win), (-1, -1), crit) + spostati = np.hypot(*(raff - grezzi).reshape(-1, 2).T) > 0.3 * passo + raff[spostati] = grezzi[spostati] # un raffinamento che scappa e' peggio del grezzo + return raff.reshape(nr, nc, 2).astype(np.float64) + + +def _estrapola(g: np.ndarray, k: int, prima: bool) -> np.ndarray: + """Aggiunge k colonne (asse 1) per estrapolazione lineare, prima o dopo.""" + if k <= 0: + return g + if prima: + passo = g[:, 0] - g[:, 1] + extra = np.stack([g[:, 0] + m * passo for m in range(k, 0, -1)], axis=1) + return np.concatenate([extra, g], axis=1) + passo = g[:, -1] - g[:, -2] + extra = np.stack([g[:, -1] + m * passo for m in range(1, k + 1)], axis=1) + return np.concatenate([g, extra], axis=1) + + +def _estendi_asse(g: np.ndarray, lungh: int, c: int, passo: float, + grid_step: int) -> tuple[np.ndarray, int]: + """Estende la griglia (asse 1) fino a coprire l'immagine; ritorna (griglia, k_prima). + + `c` e' la coordinata (0 = x, 1 = y) e `lungh` la dimensione dell'immagine su + quell'asse. `from_grid` (crop_domain=False) tiene costante la mappa fuori + dalla griglia: senza anelli in piu' i bordi dell'immagine raddrizzata + sarebbero sbavati. + """ + margine = max(0.0, float(g[:, 0, c].min())) + k_pre = int(math.ceil(margine / passo)) + 1 + g = _estrapola(g, k_pre, True) + offset = int(g[0, 0, c]) # lo stesso int() che usa from_grid + # colonne totali perche' l'ultimo nodo, a passo grid_step, superi il bordo + servono = int(math.ceil((lungh - 1 - offset) / grid_step)) + 2 + g = _estrapola(g, max(0, servono - g.shape[1]), False) + return g, k_pre + + +class MappaPronta(NamedTuple): + """Mappa gia' convertita in virgola fissa per un cv2.remap veloce (#84).""" + map1: np.ndarray + map2: np.ndarray + pixel_size: float + shape: tuple # (altezza, larghezza) dei frame per cui vale + + +def _prepara_mappa(mappa: DistortionMap) -> MappaPronta: + # CV_16SC2: remap bilineare in virgola fissa, piu' veloce di float32 su 5 MP + m1, m2 = cv2.convertMaps(mappa.map_col, mappa.map_row, cv2.CV_16SC2) + return MappaPronta(m1, m2, float(mappa.pixel_size), mappa.shape) + + +def raddrizza(frame: np.ndarray, mappa: MappaPronta) -> np.ndarray | None: + """Frame raddrizzato, o None se le dimensioni non sono quelle della calibrazione.""" + if frame.shape[:2] != tuple(mappa.shape): + return None + return cv2.remap(frame, mappa.map1, mappa.map2, cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, borderValue=0) + + +def calibra_scacchiera_densa(pixels, square_mm: float) -> tuple[dict, DistortionMap]: + """Calibra da UNA foto di scacchiera densa (riempie il campo). Input: solo square_mm. + + Ritorna (risultato, mappa). Risultato: {px_per_mm, mm_per_px (dell'immagine + RADDRIZZATA), rms_px, residuo_rms_px, residuo_max_px, n_corners, cols, rows, + square_mm, image_width, image_height, scale_spread_pct (0: la mappa uniforma + la scala), copertura_pct, metodo, distorsione, overlay_png (b64)}. + """ + if not (square_mm > 0 and math.isfinite(square_mm)): + raise CalibrazioneError("square_mm deve essere > 0") + frame = _gray_u8(pixels) + h, w = frame.shape + g = _trova_griglia(frame) + nr, nc, _ = g.shape + passo = _passo_mediano(g) + grid_step = int(round(passo)) + if grid_step < 2: + raise CalibrazioneError("calibrazione degenere: passo della griglia troppo piccolo") + + # anelli in piu' ai bordi (x e poi y), poi la mappa dai punti estesi + e, kl = _estendi_asse(g, w, 0, passo, grid_step) + et, kt = _estendi_asse(e.transpose(1, 0, 2), h, 1, passo, grid_step) + e = et.transpose(1, 0, 2) + try: + mappa = DistortionMap.from_grid( + e[..., 0].ravel(), e[..., 1].ravel(), e.shape[1], e.shape[0], + image_size=(w, h), grid_step=grid_step, crop_domain=False, + grid_step_measure=square_mm) + except ValueError as exc: + raise CalibrazioneError(f"mappa di distorsione non costruibile: {exc}") from exc + mm_per_px = float(mappa.pixel_size) + if not (mm_per_px > 0 and math.isfinite(mm_per_px)): + raise CalibrazioneError("calibrazione degenere: scala non valida") + + # qualita': si raddrizza la stessa foto e si rilegge ogni corner nel punto ideale + off_c, off_r = int(e[0, 0, 0]), int(e[0, 0, 1]) + ideali = np.array([[(off_c + (kl + j) * grid_step, off_r + (kt + i) * grid_step) + for j in range(nc)] for i in range(nr)], np.float32).reshape(-1, 1, 2) + rett = cv2.remap(frame, mappa.map_col, mappa.map_row, cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, borderValue=0) + win = int(min(20, max(3, 0.3 * grid_step))) + crit = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 40, 0.001) + # solo i corner ideali dentro l'immagine raddrizzata (cornerSubPix rifiuta quelli fuori) + xy = ideali.reshape(-1, 2) + dentro = (xy[:, 0] >= 1) & (xy[:, 0] <= w - 2) & (xy[:, 1] >= 1) & (xy[:, 1] <= h - 2) + if int(dentro.sum()) < 4: + raise CalibrazioneError("griglia raddrizzata fuori dall'immagine: calibrazione non verificabile") + ideali = ideali[dentro] + letti = cv2.cornerSubPix(rett, ideali.copy(), (win, win), (-1, -1), crit) + scarto = (letti - ideali).reshape(-1, 2).astype(np.float64) + scarto -= np.median(scarto, axis=0) # int() dell'offset sposta tutto di una traslazione + dist = np.hypot(scarto[:, 0], scarto[:, 1]) + residuo_max = float(dist.max()) + residuo_rms = float(math.sqrt(float((dist ** 2).mean()))) + + # copertura: area del quadrilatero dei quattro angoli esterni / area immagine + quad = np.array([g[0, 0], g[0, -1], g[-1, -1], g[-1, 0]], np.float32) + copertura = float(cv2.contourArea(quad)) / float(w * h) * 100.0 + + # overlay: corner trovati sull'originale (ridotto a max 1600 px: 5 MP in PNG pesano) + f = min(1.0, 1600.0 / w) + bgr = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) + pts = g.reshape(-1, 1, 2).astype(np.float32) + if f < 1.0: + bgr = cv2.resize(bgr, None, fx=f, fy=f, interpolation=cv2.INTER_AREA) + pts = pts * np.float32(f) + cv2.drawChessboardCorners(bgr, (nc, nr), pts, True) + ok, png = cv2.imencode(".png", bgr) + if not ok: + raise RuntimeError("PNG dell'overlay non codificabile") + + return { + "px_per_mm": 1.0 / mm_per_px, + "mm_per_px": mm_per_px, + "rms_px": residuo_rms, + "residuo_rms_px": residuo_rms, + "residuo_max_px": residuo_max, + "n_corners": int(nr * nc), + "cols": int(nc), + "rows": int(nr), + "square_mm": float(square_mm), + "image_width": int(w), + "image_height": int(h), + "scale_spread_pct": 0.0, + "copertura_pct": copertura, + "metodo": "densa", + "distorsione": True, + "overlay_png": base64.b64encode(png.tobytes()).decode("ascii"), + }, mappa + + def ora_iso() -> str: return datetime.now(timezone.utc).isoformat(timespec="seconds") class Archivio: - """Un JSON per sorgente in `cartella`; nome file dalla sorgente ripulito.""" + """Un JSON per sorgente in `cartella`; nome file dalla sorgente ripulito. + + #84: se la calibrazione e' densa, accanto al JSON sta la mappa (stesso nome, + .npz); `leggi_mappa` la tiene in memoria finche' il file non cambia. + """ def __init__(self, cartella: Path | str): self.cartella = Path(cartella) + self._cache: dict = {} # percorso .npz -> (mtime_ns, MappaPronta) + self._lock = threading.Lock() def _file(self, sorgente: str) -> Path: # caratteri sicuri + hash dell'originale: due sorgenti diverse non si pestano @@ -115,15 +464,23 @@ class Archivio: sigla = hashlib.sha1(sorgente.encode("utf-8")).hexdigest()[:8] return self.cartella / f"{pulito[:60]}-{sigla}.json" + def _file_mappa(self, sorgente: str) -> Path: + return self._file(sorgente).with_suffix(".npz") # #84 stesso nome del JSON + def leggi(self, sorgente: str) -> dict | None: try: return json.loads(self._file(sorgente).read_text(encoding="utf-8")) except (OSError, ValueError): return None # assente o illeggibile = non calibrata - def salva(self, sorgente: str, dati: dict) -> None: + def salva(self, sorgente: str, dati: dict, mappa: DistortionMap | None = None) -> None: destinazione = self._file(sorgente) self.cartella.mkdir(parents=True, exist_ok=True) + # #84: prima la mappa (o la rimozione di una vecchia), poi il JSON che la dichiara + if mappa is not None: + self._salva_mappa(sorgente, mappa) + else: + self._togli_mappa(sorgente) # ricalibrazione planare: la vecchia mappa non vale piu' # scrittura atomica: file temporaneo nella stessa cartella, poi replace fd, tmp = tempfile.mkstemp(dir=self.cartella, suffix=".tmp") try: @@ -137,8 +494,56 @@ class Archivio: pass raise + def _salva_mappa(self, sorgente: str, mappa: DistortionMap) -> None: + destinazione = self._file_mappa(sorgente) + fd, tmp = tempfile.mkstemp(dir=self.cartella, suffix=".tmp.npz") # .npz: numpy non aggiunge nulla + os.close(fd) + try: + mappa.save(tmp) + os.replace(tmp, destinazione) # atomica come il JSON + except BaseException: + try: + os.unlink(tmp) + except OSError: + pass + raise + with self._lock: + self._cache.pop(str(destinazione), None) + + def _togli_mappa(self, sorgente: str) -> None: + destinazione = self._file_mappa(sorgente) + try: + destinazione.unlink() + except FileNotFoundError: + pass + with self._lock: + self._cache.pop(str(destinazione), None) + + def leggi_mappa(self, sorgente: str) -> MappaPronta | None: + """La mappa salvata, pronta per `raddrizza`; None se la sorgente non ne ha una.""" + percorso = self._file_mappa(sorgente) + chiave = str(percorso) + try: + mtime = percorso.stat().st_mtime_ns + except OSError: + with self._lock: + self._cache.pop(chiave, None) + return None + with self._lock: + voce = self._cache.get(chiave) + if voce is not None and voce[0] == mtime: + return voce[1] + try: + pronta = _prepara_mappa(DistortionMap.load(percorso)) + except (OSError, ValueError, KeyError): + return None # illeggibile = nessuna correzione + with self._lock: + self._cache[chiave] = (mtime, pronta) + return pronta + def elimina(self, sorgente: str) -> None: try: self._file(sorgente).unlink() except FileNotFoundError: pass + self._togli_mappa(sorgente) # #84 toglie anche la mappa