Cronaca di un fallimento: crisotilo su Enmap (3)

Per cercare di migliorare le librerie dell'amianto su Enmap ho provato ad essere piu' selettivo

Usando WorldCover Esa Map ho selezionato in QGis i build-up che sono indicati da Banda 1 Tavolozza 50

 

 ho tagliato il raster usando come base l'impronta dell'immagine Enmap con Processing → Toolbox → GDAL → Raster extraction → Clip raster by extent

 

 

 alla fine ho ottenuto un raster 0-1 in cui 1 indica la posizione dei built-up e 0 tutte le altre classiche

 

A questo punto ho convertito il raster in un tema puntuale tramite Processing → Toolbox → GDAL → Conversione raster → Pixel raster in punti

Alla fine ho un tema puntuale di tetti..a questo punto devo selezionare solo i tetti che non sono di amianto

Per questo motivo ho creato un buffer di 60 metri attorno ai punti del tema con i punti dei tetti di amianto_PRA_pubblico con Processing → Toolbox → Geometria vettoriale → Buffer

 

siamo quasi alla fine, togliendo dal tema tutti i pixel che sono esterni al buffer si puo' usare Processing → Toolbox → Estrai per posizione usando disgiunto 

Adesso abbiamo un tema di copertura non di amianto e possiamo cominciare a lavorare ad estrre le firme spettrali 

Per estrarre dei punti casuali all'interno del tema ho usato Processing Toolobox Estrazione del vettore Estrazione casuale estraendo un nuovo tema di 20000 punti 

In seguito Enmap Toolbox Spectral Library Extract spectral profiles from raster layer per avere una libreria spettrale di tetti non in amianto

Come nel precedente post ho creato un campo boolean amianto (true per i tetti in amianto, false l'ovvio opposto) ed ho unito le due librerie con 

Per unire la libreria dei tetti in amianto e quelle non in amianto si apre la libreria A, si selezionano tutti i profili si copiano in Spectral Library Viewer di Enmap Toolbox e si incollano nella libreria B

Per rendere il modello misurabile la libreria e' stata splittata in 70% train, 20% test e 10% validate 

import json, pickle

import numpy as np

import pandas as pd

import geopandas as gpd

from sklearn.ensemble import RandomForestClassifier

from sklearn.model_selection import StratifiedGroupKFold

from sklearn.metrics import (average_precision_score, roc_auc_score,

                             precision_recall_curve, precision_score,

                             recall_score, f1_score)

# ---------------- parametri ----------------

GPKG = "C:/Users/l.innocenti/Documents/MF/worldview/libreria/unione.gpkg"

LAYER = "no_amianto2"

FIELD_PROFILES = "profile"

FIELD_BG = "amianto"              # 1.0 = amianto; NaN/vuoto = background

CELL = 500                        # metri

EDGE_DROP = [(0, 550), (2400, 3000)]                 # sempre escluse

WATER_DROP = [(1300, 1500), (1750, 2150)]            # escluse solo nelle varianti "senza bande"

N_TREES = 500

SEEDS = [0, 1, 2]                 # ogni seed cambia sia lo split spaziale sia il random forest

N_FOLDS = 10

FOLDS_TRAIN, FOLDS_TEST, FOLDS_VAL = (0, 1, 2, 3, 4, 5, 6), (7, 8), (9,)

OUT_CSV = "C:/Users/l.innocenti/Documents/MF/ablazione_risultati.csv"

TRUE_VALUES = {"true", "1", "1.0", "yes", "si", "sì", "t"}

# --------------------------------------------

def parse_profile(v):

    if v is None:

        return None

    if isinstance(v, dict):

        return v

    if isinstance(v, (bytes, bytearray, memoryview)):

        b = bytes(v)

        try:

            return json.loads(b.decode("utf-8"))

        except Exception:

            return pickle.loads(b)           # solo su file tuoi, di cui ti fidi

    if isinstance(v, str):

        return json.loads(v)

    return None

def normalize_spectra(A, mode):

    if mode is None:

        return A

    n = np.linalg.norm(A, axis=1, keepdims=True)

    return A / np.clip(n, 1e-12, None)

def make_rf(seed):

    return RandomForestClassifier(

        n_estimators=N_TREES, max_features="sqrt", min_samples_leaf=3,

        class_weight="balanced_subsample", n_jobs=-1, random_state=seed)

# --- lettura ---

df = gpd.read_file(GPKG, layer=LAYER)

print("Record letti:", len(df), "| campi:", list(df.columns))

print(df[FIELD_BG].value_counts(dropna=False))

prof = [parse_profile(v) for v in df[FIELD_PROFILES]]

lengths = [len(p["y"]) if p and p.get("y") is not None else 0 for p in prof]

n_bands = max(set(lengths), key=lengths.count)

ok = np.array([l == n_bands for l in lengths])

first = next(p for p, o in zip(prof, ok) if o)

wl = np.array(first["x"], float)

bbl = np.array(first["bbl"], bool) if first.get("bbl") is not None else np.ones(n_bands, bool)

# maschera larga (con bande reintrodotte) e stretta (senza)

mask_wide = bbl.copy()

for lo, hi in EDGE_DROP:

    mask_wide &= ~((wl >= lo) & (wl <= hi))

mask_narrow = mask_wide.copy()

for lo, hi in WATER_DROP:

    mask_narrow &= ~((wl >= lo) & (wl <= hi))

print(f"Bande: larga {mask_wide.sum()} | stretta {mask_narrow.sum()}")

X = np.array([np.asarray(p["y"], float) for p, o in zip(prof, ok) if o])

keep = np.where(ok)[0]

lab = df[FIELD_BG].iloc[keep]

y = lab.map(lambda v: 1 if str(v).strip().lower() in TRUE_VALUES else 0).to_numpy().astype(int)

# stessi campioni per tutte le varianti: filtro sulla maschera larga

Xw = X[:, mask_wide]

good = np.isfinite(Xw).all(axis=1) & (np.abs(Xw).sum(axis=1) > 0)

X, y, keep, Xw = X[good], y[good], keep[good], Xw[good]

assert len(Xw) == len(y) == len(keep), "Spettri e etichette non allineati"

print(f"Campioni: {len(y)} | amianto: {y.sum()} | background: {(y == 0).sum()}")

if y.sum() == 0 or (y == 0).sum() == 0:

    raise SystemExit("Una sola classe presente: controlla FIELD_BG")

# indici delle colonne della maschera stretta dentro la larga

sub_narrow = mask_narrow[mask_wide]

# --- gruppi spaziali ---

g = df.iloc[keep]

if g.crs is not None and g.crs.is_geographic:

    g = g.to_crs(32632)

cx = (g.geometry.x.to_numpy() // CELL).astype(np.int64)

cy = (g.geometry.y.to_numpy() // CELL).astype(np.int64)

groups = -(cx * 100000 + cy) - 1

print("Celle spaziali:", len(np.unique(groups)))

configs = [("grezzo", None, False), ("grezzo", None, True),

           ("L2", "l2", False), ("L2", "l2", True)]

rows = []

for seed in SEEDS:

    sgkf = StratifiedGroupKFold(n_splits=N_FOLDS, shuffle=True, random_state=seed)

    fold_id = np.zeros(len(y), int)

    for k, (_, te) in enumerate(sgkf.split(X, y, groups)):

        fold_id[te] = k

    tr = np.where(np.isin(fold_id, FOLDS_TRAIN))[0]

    te = np.where(np.isin(fold_id, FOLDS_TEST))[0]

    va = np.where(np.isin(fold_id, FOLDS_VAL))[0]

    for norm_name, norm, wide in configs:

        Xc = Xw if wide else Xw[:, sub_narrow]

        Xc = normalize_spectra(Xc, norm)

        rf = make_rf(seed).fit(Xc[tr], y[tr])

        pv = rf.predict_proba(Xc[va])[:, 1]

        pt = rf.predict_proba(Xc[te])[:, 1]

        prec, rec, thr = precision_recall_curve(y[va], pv)

        f1c = 2 * prec[:-1] * rec[:-1] / np.clip(prec[:-1] + rec[:-1], 1e-9, None)

        t = float(thr[f1c.argmax()])

        yp = (pt >= t).astype(int)

        rows.append({

            "seed": seed, "spettri": norm_name,

            "bande_1300-1500/1750-2150": "incluse" if wide else "escluse",

            "n_bande": Xc.shape[1],

            "PR-AUC": average_precision_score(y[te], pt),

            "ROC-AUC": roc_auc_score(y[te], pt),

            "soglia": t,

            "precision": precision_score(y[te], yp, zero_division=0),

            "recall": recall_score(y[te], yp),

            "F1": f1_score(y[te], yp),

        })

        r = rows[-1]

        print(f"seed {seed} | {norm_name:6s} | bande {r['bande_1300-1500/1750-2150']:7s} "

              f"| PR-AUC {r['PR-AUC']:.3f} | ROC-AUC {r['ROC-AUC']:.3f} | F1 {r['F1']:.3f}")

res = pd.DataFrame(rows)

res.to_csv(OUT_CSV, index=False)

keys = ["spettri", "bande_1300-1500/1750-2150", "n_bande"]

metrics = ["PR-AUC", "ROC-AUC", "precision", "recall", "F1"]

summary = res.groupby(keys)[metrics].agg(["mean", "std"]).round(3)

pd.set_option("display.width", 250)

pd.set_option("display.max_columns", 50)

print(f"\n=== RISULTATI SUL TEST (media e dev. std su {len(SEEDS)} seed, CELL={CELL} m) ===")

print(summary)

print(f"\nBaseline PR-AUC (prevalenza amianto): {y.mean():.3f}")

print("Risultati per singolo seed salvati in", OUT_CSV)

 

import json, pickle

import numpy as np

import pandas as pd

import geopandas as gpd

from sklearn.ensemble import RandomForestClassifier

from sklearn.model_selection import StratifiedGroupKFold

from sklearn.metrics import (average_precision_score, roc_auc_score,

                             precision_recall_curve, precision_score,

                             recall_score, f1_score)

# ---------------- parametri ----------------

GPKG = "C:/Users/l.innocenti/Documents/MF/worldview/libreria/unione.gpkg"

LAYER = "no_amianto2"

FIELD_PROFILES = "profile"

FIELD_BG = "amianto"              # 1.0 = amianto; NaN/vuoto = background

CELL = 500                        # metri

EDGE_DROP = [(0, 550), (2400, 3000)]                 # sempre escluse

WATER_DROP = [(1300, 1500), (1750, 2150)]            # escluse solo nelle varianti "senza bande"

N_TREES = 500

SEEDS = [0, 1, 2]                 # ogni seed cambia sia lo split spaziale sia il random forest

N_FOLDS = 10

FOLDS_TRAIN, FOLDS_TEST, FOLDS_VAL = (0, 1, 2, 3, 4, 5, 6), (7, 8), (9,)

OUT_CSV = "C:/Users/l.innocenti/Documents/MF/ablazione_risultati.csv"

TRUE_VALUES = {"true", "1", "1.0", "yes", "si", "sì", "t"}

# --------------------------------------------

def parse_profile(v):

    if v is None:

        return None

    if isinstance(v, dict):

        return v

    if isinstance(v, (bytes, bytearray, memoryview)):

        b = bytes(v)

        try:

            return json.loads(b.decode("utf-8"))

        except Exception:

            return pickle.loads(b)           # solo su file tuoi, di cui ti fidi

    if isinstance(v, str):

        return json.loads(v)

    return None

def normalize_spectra(A, mode):

    if mode is None:

        return A

    n = np.linalg.norm(A, axis=1, keepdims=True)

    return A / np.clip(n, 1e-12, None)

def make_rf(seed):

    return RandomForestClassifier(

        n_estimators=N_TREES, max_features="sqrt", min_samples_leaf=3,

        class_weight="balanced_subsample", n_jobs=-1, random_state=seed)

# --- lettura ---

df = gpd.read_file(GPKG, layer=LAYER)

print("Record letti:", len(df), "| campi:", list(df.columns))

print(df[FIELD_BG].value_counts(dropna=False))

prof = [parse_profile(v) for v in df[FIELD_PROFILES]]

lengths = [len(p["y"]) if p and p.get("y") is not None else 0 for p in prof]

n_bands = max(set(lengths), key=lengths.count)

ok = np.array([l == n_bands for l in lengths])

first = next(p for p, o in zip(prof, ok) if o)

wl = np.array(first["x"], float)

bbl = np.array(first["bbl"], bool) if first.get("bbl") is not None else np.ones(n_bands, bool)

# maschera larga (con bande reintrodotte) e stretta (senza)

mask_wide = bbl.copy()

for lo, hi in EDGE_DROP:

    mask_wide &= ~((wl >= lo) & (wl <= hi))

mask_narrow = mask_wide.copy()

for lo, hi in WATER_DROP:

    mask_narrow &= ~((wl >= lo) & (wl <= hi))

print(f"Bande: larga {mask_wide.sum()} | stretta {mask_narrow.sum()}")

X = np.array([np.asarray(p["y"], float) for p, o in zip(prof, ok) if o])

keep = np.where(ok)[0]

lab = df[FIELD_BG].iloc[keep]

y = lab.map(lambda v: 1 if str(v).strip().lower() in TRUE_VALUES else 0).to_numpy().astype(int)

# stessi campioni per tutte le varianti: filtro sulla maschera larga

Xw = X[:, mask_wide]

good = np.isfinite(Xw).all(axis=1) & (np.abs(Xw).sum(axis=1) > 0)

X, y, keep, Xw = X[good], y[good], keep[good], Xw[good]

assert len(Xw) == len(y) == len(keep), "Spettri e etichette non allineati"

print(f"Campioni: {len(y)} | amianto: {y.sum()} | background: {(y == 0).sum()}")

if y.sum() == 0 or (y == 0).sum() == 0:

    raise SystemExit("Una sola classe presente: controlla FIELD_BG")

# indici delle colonne della maschera stretta dentro la larga

sub_narrow = mask_narrow[mask_wide]

# --- gruppi spaziali ---

g = df.iloc[keep]

if g.crs is not None and g.crs.is_geographic:

    g = g.to_crs(32632)

cx = (g.geometry.x.to_numpy() // CELL).astype(np.int64)

cy = (g.geometry.y.to_numpy() // CELL).astype(np.int64)

groups = -(cx * 100000 + cy) - 1

print("Celle spaziali:", len(np.unique(groups)))

configs = [("grezzo", None, False), ("grezzo", None, True),

           ("L2", "l2", False), ("L2", "l2", True)]

rows = []

for seed in SEEDS:

    sgkf = StratifiedGroupKFold(n_splits=N_FOLDS, shuffle=True, random_state=seed)

    fold_id = np.zeros(len(y), int)

    for k, (_, te) in enumerate(sgkf.split(X, y, groups)):

        fold_id[te] = k

    tr = np.where(np.isin(fold_id, FOLDS_TRAIN))[0]

    te = np.where(np.isin(fold_id, FOLDS_TEST))[0]

    va = np.where(np.isin(fold_id, FOLDS_VAL))[0]

    for norm_name, norm, wide in configs:

        Xc = Xw if wide else Xw[:, sub_narrow]

        Xc = normalize_spectra(Xc, norm)

        rf = make_rf(seed).fit(Xc[tr], y[tr])

        pv = rf.predict_proba(Xc[va])[:, 1]

        pt = rf.predict_proba(Xc[te])[:, 1]

        prec, rec, thr = precision_recall_curve(y[va], pv)

        f1c = 2 * prec[:-1] * rec[:-1] / np.clip(prec[:-1] + rec[:-1], 1e-9, None)

        t = float(thr[f1c.argmax()])

        yp = (pt >= t).astype(int)

        rows.append({

            "seed": seed, "spettri": norm_name,

            "bande_1300-1500/1750-2150": "incluse" if wide else "escluse",

            "n_bande": Xc.shape[1],

            "PR-AUC": average_precision_score(y[te], pt),

            "ROC-AUC": roc_auc_score(y[te], pt),

            "soglia": t,

            "precision": precision_score(y[te], yp, zero_division=0),

            "recall": recall_score(y[te], yp),

            "F1": f1_score(y[te], yp),

        })

        r = rows[-1]

        print(f"seed {seed} | {norm_name:6s} | bande {r['bande_1300-1500/1750-2150']:7s} "

              f"| PR-AUC {r['PR-AUC']:.3f} | ROC-AUC {r['ROC-AUC']:.3f} | F1 {r['F1']:.3f}")

res = pd.DataFrame(rows)

res.to_csv(OUT_CSV, index=False)

keys = ["spettri", "bande_1300-1500/1750-2150", "n_bande"]

metrics = ["PR-AUC", "ROC-AUC", "precision", "recall", "F1"]

summary = res.groupby(keys)[metrics].agg(["mean", "std"]).round(3)

pd.set_option("display.width", 250)

pd.set_option("display.max_columns", 50)

print(f"\n=== RISULTATI SUL TEST (media e dev. std su {len(SEEDS)} seed, CELL={CELL} m) ===")

print(summary)

print(f"\nBaseline PR-AUC (prevalenza amianto): {y.mean():.3f}")

print("Risultati per singolo seed salvati in", OUT_CSV)

 

In conclusione il modello ha trovato 642 tetti in amianto su 1014 (ne ha mancati 372) e ha segnalato per errore 806 pixel di background su 2542. 

Precision = TP / (TP + FP): 0.73 (tra i pixel che il modello segnala come amianto, quanti lo sono davvero) 

Recall = TP / (TP + FN): 0.057 (pixel che sono amianto, quanti il modello ne trova)

Bande più importanti:
banda 1449: 0.0112
banda 1128: 0.0099
banda 2225: 0.0089
banda 1140: 0.0085
banda 1996: 0.0083
banda 2216: 0.0081
banda 1152: 0.0079
banda 951: 0.0079
banda 941: 0.0079
banda 1259: 0.0078

 

Icosaedro Aruco Tags

Questo test prende un icosaedro sul quale sono stati apposti degli aruco tags (in modo da avere almeno 3 facce sempre visibili alla camera) e tramite le normali calcola la posizione del centro dell'icosaedro tramite opencv

Un limite di questo metodo nel costruirselo da soli e' che e' difficile centrare il centro dell'aruco tag sul centro della faccia...e questo limita molto la precisione del posizionamento del centro dell'icosaedro dato che e' calcolata dall'intersezione delle normali agli aruco tags 

 

 

 

"""

Calibrazione della camera RGB della RealSense D415 con scacchiera.

Requisiti:

pip install pyrealsense2 opencv-contrib-python numpy

Uso:

1. Stampa checkerboard_A4.pdf A SCALA 100% (non "adatta alla pagina").

2. Misura col righello il quadratino di verifica stampato sul foglio:

se non è esattamente SQUARE_SIZE_M metri di lato, correggi la costante

qui sotto con la misura reale.

3. Esegui lo script, inquadra la scacchiera da tante angolazioni/distanze/

inclinazioni diverse (coprendo tutta l'inquadratura: centro, angoli,

vicino, lontano, inclinata). Premi SPAZIO per catturare un frame quando

gli angoli vengono rilevati (disegnati a colori), ESC/'q' per terminare

e calibrare.

4. Servono almeno 15-20 catture buone, distribuite in tutta l'immagine,

per una calibrazione affidabile.

Il risultato (camera_matrix, dist_coeffs) viene salvato in

d415_calibration.npz, pronto per essere caricato nello script ArUco.

"""

import cv2

import numpy as np

import pyrealsense2 as rs

# ------------------------- CONFIGURAZIONE -------------------------

# Angoli interni della scacchiera (colonne, righe) = (num_quadrati_x - 1, num_quadrati_y - 1)

PATTERN_SIZE = (9, 6)

# Lato di un quadrato in METRI: usa il valore misurato col righello sul foglio stampato

SQUARE_SIZE_M = 0.020

FRAME_W, FRAME_H, FPS = 1280, 720, 30

OUT_NPZ = "d415_calibration.npz"

MIN_CAPTURES = 15

def build_object_points(pattern_size, square_size):

cols, rows = pattern_size

objp = np.zeros((rows * cols, 3), np.float32)

objp[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) * square_size

return objp

def main():

objp_template = build_object_points(PATTERN_SIZE, SQUARE_SIZE_M)

pipeline = rs.pipeline()

config = rs.config()

config.enable_stream(rs.stream.color, FRAME_W, FRAME_H, rs.format.bgr8, FPS)

profile = pipeline.start(config)

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

obj_points = [] # punti 3D reali (nel piano della scacchiera)

img_points = [] # punti 2D corrispondenti nell'immagine

img_size = None

print("SPAZIO = cattura frame corrente, 'q'/ESC = termina e calibra")

try:

while True:

frames = pipeline.wait_for_frames()

color_frame = frames.get_color_frame()

if not color_frame:

continue

frame = np.asanyarray(color_frame.get_data())

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

img_size = gray.shape[::-1] # (w, h)

found, corners = cv2.findChessboardCorners(

gray, PATTERN_SIZE,

flags=cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE

)

display = frame.copy()

if found:

corners_refined = cv2.cornerSubPix(

gray, corners, (11, 11), (-1, -1), criteria

)

cv2.drawChessboardCorners(display, PATTERN_SIZE, corners_refined, found)

else:

corners_refined = None

cv2.putText(display, f"Catture: {len(obj_points)}/{MIN_CAPTURES}+",

(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7,

(0, 255, 0) if found else (0, 0, 255), 2)

cv2.imshow("Calibrazione D415 - SPAZIO=cattura, q=fine", display)

key = cv2.waitKey(1) & 0xFF

if key == ord(' ') and found:

obj_points.append(objp_template.copy())

img_points.append(corners_refined)

print(f"Cattura #{len(obj_points)} registrata")

elif key in (ord('q'), 27):

break

finally:

pipeline.stop()

cv2.destroyAllWindows()

if len(obj_points) < 4:

print("Troppo poche catture per calibrare (minimo consigliato 15-20).")

return

print(f"Calibrazione in corso con {len(obj_points)} catture...")

ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(

obj_points, img_points, img_size, None, None

)

# Errore di riproiezione medio, indicatore della qualità della calibrazione.

# Uso numpy invece di cv2.norm: su alcune build di OpenCV (es. 5.0) cv2.norm

# può lamentare un mismatch di tipo (CV_32FC1 vs CV_32FC2) anche se le shape

# sono corrette, quindi si aggira il problema calcolando la norma a mano.

total_error = 0

for i in range(len(obj_points)):

proj, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], camera_matrix, dist_coeffs)

proj = np.asarray(proj, dtype=np.float64).reshape(-1, 2)

observed = np.asarray(img_points[i], dtype=np.float64).reshape(-1, 2)

error = np.linalg.norm(observed - proj, axis=1).mean()

total_error += error

mean_error = total_error / len(obj_points)

print("RMS calibrazione:", ret)

print("Errore di riproiezione medio (px, minore è meglio, <0.5 è ottimo):", mean_error)

print("Camera matrix:\n", camera_matrix)

print("Distortion coeffs:\n", dist_coeffs.ravel())

np.savez(OUT_NPZ, camera_matrix=camera_matrix, dist_coeffs=dist_coeffs,

image_size=img_size, mean_reproj_error=mean_error)

print(f"Salvato in {OUT_NPZ}")

if __name__ == "__main__":

main()

 

"""

Stima della posizione 3D del centro di un icosaedro con marker ArUco sulle facce,

tramite intersezione ai minimi quadrati delle normali dei marker rilevati

in uno stream video dalla RealSense D415 (canale RGB), via pyrealsense2 -

stesso approccio usato in calibrate_d415.py, che negozia la risoluzione con

la camera in modo affidabile (a differenza di cv2.VideoCapture generico via

UVC/V4L2, che spesso resta bloccato a risoluzioni basse). Il punto stimato

viene mediato sugli ultimi 10 frame per ridurre il jitter.

Richiede: pyrealsense2, opencv-contrib-python (per il modulo cv2.aruco), numpy

pip install pyrealsense2 opencv-contrib-python numpy

"""

from collections import deque

import cv2

import numpy as np

import pyrealsense2 as rs

# ------------------------- CONFIGURAZIONE -------------------------

# Calibrazione camera: SOSTITUISCI con i valori reali della tua camera

# (ottenuti con calibrate_d415.py). Senza calibrazione corretta la stima

# del centro sarà sbagliata.

CAMERA_MATRIX = np.array([

[931.62977526, 0.0, 648.9939992],

[ 0.0, 935.13433783, 366.09135937],

[ 0.0, 0.0, 1.0]

], dtype=np.float64)

DIST_COEFFS = np.zeros((5, 1), dtype=np.float64)

# Risoluzione/fps del canale RGB richiesti alla D415. Deve combaciare con la

# risoluzione usata in calibrate_d415.py (FRAME_W/FRAME_H), altrimenti

# fx/fy/cx/cy non sono più validi.

FRAME_W, FRAME_H, FPS = 1280, 720, 30

MARKER_LENGTH = 0.026 # lato del marker in metri: misuralo con un calibro

ARUCO_DICT = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)

DETECTOR_PARAMS = cv2.aruco.DetectorParameters()

DETECTOR = cv2.aruco.ArucoDetector(ARUCO_DICT, DETECTOR_PARAMS)

# Se true, inverte il segno della normale (dipende da come è orientato

# il marker rispetto alla faccia: prova entrambi i valori e guarda quale

# fa convergere il punto rosso dentro il solido)

FLIP_NORMAL = True

# Numero di stime consecutive su cui calcolare la media mobile del centro

SMOOTHING_WINDOW = 10

WINDOW_NAME = "Icosaedro - stima centro 3D"

def marker_object_points(length):

"""Punti 3D degli angoli del marker nel suo sistema locale, piano z=0,

ordine: alto-sx, alto-dx, basso-dx, basso-sx (ordine standard ArUco)."""

h = length / 2.0

return np.array([

[-h, h, 0],

[ h, h, 0],

[ h, -h, 0],

[-h, -h, 0]

], dtype=np.float64)

OBJ_POINTS = marker_object_points(MARKER_LENGTH)

def estimate_marker_pose(corners, camera_matrix):

"""Pose di un singolo marker via solvePnP (sostituisce l'estimatePoseSingleMarkers

ormai deprecato). Ritorna rvec, tvec."""

ok, rvec, tvec = cv2.solvePnP(

OBJ_POINTS, corners.reshape(-1, 2), camera_matrix, DIST_COEFFS,

flags=cv2.SOLVEPNP_IPPE_SQUARE

)

return rvec, tvec

def closest_point_to_lines(points, directions):

"""

Trova il punto 3D che minimizza la somma delle distanze quadratiche da un

insieme di rette skew, ciascuna definita da (punto, direzione unitaria).

Risolve A x = b con A = sum(I - d d^T), b = sum((I - d d^T) p).

"""

A = np.zeros((3, 3))

b = np.zeros(3)

I = np.eye(3)

for p, d in zip(points, directions):

d = d / np.linalg.norm(d)

M = I - np.outer(d, d)

A += M

b += M @ p

x, *_ = np.linalg.lstsq(A, b, rcond=None)

return x

def open_realsense_color_stream():

"""Apre il pipeline RealSense sul solo canale colore, come in

calibrate_d415.py. Ritorna il pipeline avviato."""

pipeline = rs.pipeline()

config = rs.config()

config.enable_stream(rs.stream.color, FRAME_W, FRAME_H, rs.format.bgr8, FPS)

profile = pipeline.start(config)

color_profile = rs.video_stream_profile(profile.get_stream(rs.stream.color))

intr = color_profile.get_intrinsics()

print(f"Stream RGB D415 avviato: {intr.width}x{intr.height}@{FPS}fps")

if (intr.width, intr.height) != (FRAME_W, FRAME_H):

print("ATTENZIONE: la risoluzione effettiva riportata dalla camera "

f"({intr.width}x{intr.height}) non combacia con quella richiesta "

f"({FRAME_W}x{FRAME_H}). Verifica che FRAME_W/FRAME_H siano supportati "

"da questo sensore (realsense-viewer per l'elenco modalità).")

return pipeline

def main():

pipeline = open_realsense_color_stream()

# Finestra ridimensionabile dall'utente (trascinando i bordi)

cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)

sign = -1.0 if FLIP_NORMAL else 1.0

# Buffer circolare con gli ultimi N centri stimati, per la media mobile

center_history = deque(maxlen=SMOOTHING_WINDOW)

try:

while True:

frames = pipeline.wait_for_frames()

color_frame = frames.get_color_frame()

if not color_frame:

continue

frame = np.asanyarray(color_frame.get_data())

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

corners, ids, _ = DETECTOR.detectMarkers(gray)

line_points, line_dirs = [], []

if ids is not None:

cv2.aruco.drawDetectedMarkers(frame, corners, ids)

for c in corners:

rvec, tvec = estimate_marker_pose(c, CAMERA_MATRIX)

R, _ = cv2.Rodrigues(rvec)

# Normale della faccia in coordinate camera (asse Z locale del marker)

normal = R @ np.array([0.0, 0.0, 1.0])

center = tvec.reshape(3)

# Il centro dell'icosaedro sta lungo la normale, dalla parte

# opposta a dove punta il marker (verso "dentro" al solido)

line_points.append(center)

line_dirs.append(sign * normal)

cv2.drawFrameAxes(frame, CAMERA_MATRIX, DIST_COEFFS, rvec, tvec, MARKER_LENGTH * 0.5)

if len(line_points) >= 2:

center3d = closest_point_to_lines(line_points, line_dirs)

center_history.append(center3d)

# Media mobile sugli ultimi SMOOTHING_WINDOW valori disponibili

center_avg = np.mean(center_history, axis=0)

img_pt, _ = cv2.projectPoints(

center_avg.reshape(1, 3), np.zeros(3), np.zeros(3),

CAMERA_MATRIX, DIST_COEFFS

)

x, y = img_pt.ravel().astype(int)

cv2.circle(frame, (x, y), 6, (0, 0, 255), -1)

cv2.putText(

frame,

f"centro (media {len(center_history)}/{SMOOTHING_WINDOW}): "

f"{center_avg[0]:.3f}, {center_avg[1]:.3f}, {center_avg[2]:.3f} m",

(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2

)

else:

cv2.putText(frame, "servono >=2 marker visibili", (10, 30),

cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 165, 255), 2)

cv2.imshow(WINDOW_NAME, frame)

if cv2.waitKey(1) & 0xFF == ord('q'):

break

finally:

pipeline.stop()

cv2.destroyAllWindows()

if __name__ == "__main__":

main()