Guida introduttiva a PnP: recupero della posizione della telecamera a partire da soli punti 3D e un'immagine.

Supponiamo che un'immagine ripresa da una telecamera abbia diversi punti 3D noti su una mappa che corrispondono a punti nell'immagine stessa. Il problema di determinare la posizione della telecamera e la sua direzione di puntamento è il problema PnP (Perspective-n-Point). Questo metodo è comunemente utilizzato nel tracciamento di mappe per Visual SLAM, nella sovrapposizione di oggetti virtuali in AR, nella calibrazione occhio-mano per i robot e nella stima della posa per le telecamere di rilevamento.

0. Riepilogo di 30 secondi

• L'input è costituito dalla matrice intrinseca della telecamera K, dai punti 3D noti \mathbf X_i e dai corrispondenti punti immagine \mathbf u_i. L'output è una rotazione R e una traslazione t.

• Il problema minimizza l'errore di riproiezione dell'equazione di proiezione \mathbf u_i\sim K(R\mathbf X_i+t). Con 3 punti, P3P fornisce soluzioni candidate; con 4 o più punti, la ridondanza consente di rilevare valori anomali.

• EPnP esprime ciascun punto come una combinazione lineare di 4 punti di controllo virtuali, risolvendo rapidamente il problema per molti punti. Un'ottimizzazione non lineare finale, come Levenberg-Marquardt, affina poi il risultato.

• Se i valori anomali si mescolano alle corrispondenze, l'intera stima della posa può collassare, quindi viene verificata con RANSAC-PnP, controlli di profondità positiva e coerenza fotogramma per fotogramma.

• Degenerazione e divergenza sono comuni quando i punti sono quasi complanari, la parallasse è piccola, i parametri intrinseci sono errati o sono presenti rolling shutter o oggetti dinamici nella scena.

1. Il modello di proiezione

Diagram 1 · Use the button to switch views

Figura 1 — Gli ID corrispondenti definiscono le corrispondenze 3D-2D. PnP formula un'ipotesi di posa, scarta i valori anomali tramite il residuo di riproiezione e raffina R,t, che mappa le coordinate del mondo in coordinate della telecamera.

Sia \mathbf X_c=R\mathbf X_w+t un punto nel sistema di coordinate della telecamera. Nel modello a foro stenopeico, le coordinate normalizzate dell'immagine sono

e le coordinate dei pixel si ottengono tramite la matrice intrinseca

come \mathbf u\sim K\mathbf X_c. R\in SO(3) rappresenta la rotazione e t la traslazione. In presenza di distorsione dell'obiettivo, è necessaria una correzione della distorsione prima e dopo la proiezione.

Le incognite sono i 6 gradi di libertà: 3 di rotazione più 3 di traslazione. Date le n corrispondenze tra i punti 3D \mathbf X_i e le osservazioni \mathbf u_i, l'errore di riproiezione

viene minimizzato. \pi rappresenta la divisione prospettica e \rho è una funzione di perdita robusta come quella di Huber.

Non confondere la trasformazione con la posizione della telecamera

OpenCV solvePnP restituisce rvec, tvec per la trasformazione che mappa i punti oggetto/mondo nel sistema di riferimento della telecamera. Per ottenere il centro della telecamera in coordinate mondo, utilizzare \mathbf C_w=-R^Tt; per una posa della telecamera, invertire T_{cw} per ottenere T_{wc}. Trattare tvec stesso come la posizione mondo della telecamera è un errore comune. Evitare inoltre di applicare la distorsione due volte quando i punti dell'immagine di input sono già stati corretti.

2. P3P, AP3P e EPnP

P3P (Perspective-3-Point), che recupera la distanza dal centro della telecamera utilizzando gli angoli dell'immagine di 3 punti e le distanze tra i punti 3D, ha fino a 4 soluzioni. Quella corretta viene selezionata confrontandola con un quarto punto o con la posa nota della mappa. AP3P è una variante veloce che riorganizza il soluzione algebricamente.

Quando ci sono molti punti, EPnP (Efficient PnP) esprime ogni punto 3D come una somma ponderata di 4 punti di controllo virtuali.

Le coordinate della telecamera dei punti di controllo vengono ricavate da equazioni lineari, e da queste si ricavano la rotazione e la traslazione. Poiché il suo costo computazionale è quasi lineare rispetto al numero di punti, è particolarmente adatto per costruire una posa iniziale a partire dai numerosi punti di riferimento di SLAM. Dopo la soluzione iniziale, l'errore di riproiezione viene affinato iterativamente con l'algoritmo di Levenberg-Marquardt.

3. RANSAC-PnP

Le corrispondenze tra punti caratteristici si mescolano con pattern simili, oggetti in movimento e ID di mappa errati. L'approccio standard è RANSAC: costruire una posa provvisoria da un insieme minimo di punti, riproiettare ogni corrispondenza e contare quanti punti inlier rientrano entro una determinata soglia. Il numero di iterazioni necessarie, dato il tasso di outlier, viene calcolato. \epsilon, la dimensione minima del campione s e la probabilità di successo p sono determinate da

Poiché il numero di iterazioni richieste cresce rapidamente con il tasso di outlier, è necessario ridurre \epsilon in anticipo utilizzando un test del rapporto tra punti caratteristici, la dispersione basata su griglia o una maschera di oggetti dinamici. solvePnPRansac di OpenCV viene utilizzato specificando esplicitamente il conteggio dei punti, i flag (EPNP, P3P, SQPNP, ecc.), la soglia di riproiezione e la confidenza.

4. Individuazione della degenerazione

Insiemi di punti complanari

Se tutti i punti 3D giacciono sullo stesso piano, la profondità e la posa da PnP diventano ambigue e la geometria può essere ugualmente spiegata da un'omografia. Calibrazione a scacchiera Utilizza deliberatamente un piano, ma è necessario scegliere punti di vista e layout dei punti che limitino sufficientemente i gradi di libertà della posa. Un singolo marker AR visto frontalmente che diventa instabile in profondità è lo stesso fenomeno.

Copertura dell'immagine ristretta e lungo raggio

Il PnP utilizza direttamente un'immagine e punti 3D noti, quindi la parallasse tra i frame non è di per sé un input richiesto. Tuttavia, è mal condizionato quando le corrispondenze occupano solo una piccola regione dell'immagine, il bersaglio è distante e appare piccolo, o i punti 3D hanno poca variazione di profondità. Non accettare un risultato basandoti solo sul conteggio degli inlier: ispeziona la copertura dell'immagine, l'RMSE di riproiezione e la covarianza della posa o la sensibilità alle perturbazioni, quindi integra un IMU o un sensore di profondità quando necessario.

Calibrazione e temporizzazione

Gli errori nella lunghezza focale, nel punto principale e nella distorsione si trasformano in un errore di riproiezione sistematico su ogni punto. Un obiettivo la cui matrice intrinseca cambia con lo zoom, la temperatura o la messa a fuoco deve essere ricalibrato. Nei veicoli e nei droni, Se la temporizzazione delle righe di un rolling shutter non è sincronizzata con l'IMU, PnP restituirà una posa della telecamera "piegata".

5. Il ruolo di PnP in Visual SLAM

In Visual SLAM, man mano che aumenta il numero di punti della mappa precedentemente triangolati, la posa della telecamera può essere tracciata fotogramma per fotogramma con PnP. Mantenendo la posa fissa, vengono triangolati nuovi punti e, una volta accumulati un numero sufficiente di fotogrammi chiave, Bundle Adjustment ottimizza congiuntamente la posa e la mappa. È più facile comprenderlo come una divisione del lavoro: PnP è il front-end leggero e Bundle Adjustment gestisce la coerenza globale.

6. Lista di controllo per l'implementazione

1. Calibrare K e la distorsione con una scacchiera o simile e registrare l'errore di riproiezione.

2. Allineare le unità (m/mm) e il sistema di coordinate dei punti 3D con lo stato di correzione della distorsione dei punti immagine.

3. Restringere le corrispondenze con un test di rapporto, mutua vicini più prossimi e tracciamento temporale.

4. Rimuovere gli outlier con RANSAC-PnP e salvare la distribuzione degli inlier e l'errore di riproiezione.

5. Verificare che la profondità sia positiva, che il cambiamento di posa sia fisicamente plausibile e che la differenza rispetto al frame precedente sia ragionevole.

6. Se le condizioni sono sfavorevoli, ricorrere a un IMU, alla profondità, a un'omografia o alla reinizializzazione.

Sequenza di implementazione minima

Con OpenCV, ottenere prima rvec, tvec, inliers da solvePnPRansac, passare solo gli inlier a solvePnPRefineLM e infine utilizzare projectPoints per calcolare autonomamente l'RMSE degli inlier e la distribuzione dell'errore. Un semplice ritorno API positivo non rileva discrepanze eccessive, punti raggruppati o una posa fisicamente impossibile.

ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, K, dist,
    flags=cv2.SOLVEPNP_EPNP,
    reprojectionError=3.0, confidence=0.999, iterationsCount=200,
)
if not ok or inliers is None or len(inliers) < 6:
    raise RuntimeError("PnP failed or has too few inliers")

idx = inliers.ravel()
rvec, tvec = cv2.solvePnPRefineLM(
    object_points[idx], image_points[idx], K, dist, rvec, tvec
)
projected, _ = cv2.projectPoints(object_points[idx], rvec, tvec, K, dist)
rmse = np.sqrt(np.mean(np.sum(
    (projected.reshape(-1, 2) - image_points[idx].reshape(-1, 2)) ** 2,
    axis=1,
)))
R, _ = cv2.Rodrigues(rvec)
camera_center_world = -R.T @ tvec.reshape(3, 1)

Nemmeno 3.0 px Né sei punti inlier rappresentano una soglia di accettazione universale; sono solo valori di partenza per questo esempio. Deriva le soglie dalla risoluzione dell'immagine, dalla precisione delle caratteristiche e dall'errore di posa consentito dall'applicazione. Verifica inoltre che i punti inlier non siano raggruppati in un angolo dell'immagine e che ogni punto abbia una profondità di campo pari a Z_c>0.

7. Riepilogo

PnP è il ponte che converte le corrispondenze tra una mappa 3D e un'immagine 2D in una posa della telecamera con 6 gradi di libertà. Crea una soluzione iniziale con P3P/EPnP, rimuovi i punti anomali con RANSAC e perfeziona con l'ottimizzazione non lineare. Solo gestendo insieme il layout dei punti, la calibrazione, la parallasse e la sincronizzazione temporale si ottiene una stima di posa stabile per Visual SLAM o AR.

Riferimenti

• OpenCV — Calibrazione della telecamera e ricostruzione 3D
• Lepetit et al., EPnP
• Gao et al., Classificazione completa della soluzione per il problema prospettico a tre punti
• Hartley e Zisserman, Geometria a viste multiple