Il termine SLAM (Simultaneous Localization And Mapping) indica il problema di costruire una mappa, localizzando al tempo stesso un robot all'interno di quella mappa. Due problemi complementari si intrecciano:
Per la localizzazione si distinguono due problemi tipici: il pose tracking (la posizione iniziale è nota e va raffinata nel tempo) e la global localization (la posizione iniziale è ignota e va determinata da zero). Il legame tra i due problemi è circolare: per una localizzazione accurata è necessaria una buona mappa, ma la costruzione di una buona mappa richiede una stima accurata della posa — è esattamente questa dipendenza reciproca che rende lo SLAM un problema difficile.
Le applicazioni sono numerose: robot domestici (aspirapolvere, robot di servizio), industria, realtà aumentata e la localizzazione mobile su larga scala (es. il sistema SLAM mobile di Facebook per la navigazione indoor).
Negli ultimi decenni sono stati studiati numerosi approcci allo SLAM basati su sensori di varia natura: IMU, sonar, IR, laser. Il Visual SLAM è lo SLAM basato su sensori visivi (camere, camere RGB-D, ecc.). I vantaggi dell'informazione visiva: è molto ricca rispetto ai dati degli altri sensori, è acquisibile con dispositivi di costo limitato (anche uno smartphone), ma comporta una maggiore complessità computazionale per le applicazioni real-time. Esistono numerosi progetti open source di riferimento: RTAB-Map, LSD-SLAM, ORB-SLAM, SVO e ElasticFusion.
La scelta del sensore condiziona tutta la pipeline. La prima componente del framework (i «dati del sensore») distingue tre famiglie:
È impossibile ottenere, da singole immagini, informazioni relative alla distanza degli oggetti. È quindi necessario muovere la telecamera: si ottengono solo valori relativi e la scala è ignota (il problema della scala relativa, sezione 8).
Due telecamere sincronizzate poste a una distanza prefissata (la baseline). È possibile calcolare la posizione 3D di ogni pixel tramite triangolazione, a fronte di un'elevata complessità computazionale.
Telecamere attive a luce strutturata o Time-of-Flight: la ricostruzione 3D è operata dal sensore stesso. Limiti: range di misurazione limitato, rumore, field of view ridotto — pensate per scenari indoor.
Il framework tipico si articola in cinque componenti, che le slide riassumono in uno schema a blocchi:
L'esploratore qui sotto percorre il flusso delle informazioni: il frontend stima il movimento, il backend lo ottimizza, il loop closing fornisce un vincolo globale aggiuntivo al backend e la reconstruction produce la mappa finale.
Clicca i blocchi o segui le transizioni per percorrere il flusso dei dati: quale componente alimenta quale?
Lo SLAM può essere formulato come un problema di stima dello stato. Le assunzioni: i sensori acquisiscono dati a intervalli regolari; la sequenza di posizioni che rappresenta la traiettoria è x1, …, xk; la mappa è costituita da un insieme di N landmark y1, …, yN. Lo spostamento del robot è descritto da due componenti: la motion (il valore di x all'istante k) e la observation (il rilevamento di un landmark yj nella posizione xk).
Motion equation: x_k = f(x_{k−1}, u_k, w_k) u_k: comando in input, w_k: rumore
Observation equation: z_{k,j} = h(y_j, x_k, v_{k,j}) v_{k,j}: rumore (camera model)L'esempio delle slide per la motion equation è un robot che si muove nel piano: [x1, x2, θ]_k = [x1, x2, θ]_{k−1} + [Δx1, Δx2, Δθ]_k + w_k. Il problema che ne deriva è un state estimation problem: come stimare x (localization) e y (mapping), dati un input di controllo u e un'osservazione z? La soluzione dipende da:
Il caso più semplice è lineare con rumore Gaussiano: la soluzione ottimale è data dai Kalman Filters (KF). I casi più complessi richiedono tecniche di ottimizzazione non lineare o approcci più avanzati dei KF.
La Visual Odometry (VO) è il processo che stima incrementalmente la posizione di un veicolo esaminando i cambiamenti che il movimento induce nelle immagini acquisite da camere montate a bordo dell'oggetto in movimento. Le motivazioni principali:
La formulazione: un agente si muove acquisendo immagini a intervalli discreti k con una telecamera fissata rigidamente. La sequenza monoculare si indica I0:n = I0, …, In; per lo stereo si hanno due sequenze Il,0:n e Ir,0:n (senza perdita di generalità, il sistema di coordinate della camera sinistra è l'origine); per l'RGB-D si hanno le sequenze I0:n (colore) e D0:n (profondità).
Le posizioni della telecamera in due istanti adiacenti k−1 e k sono collegate da una trasformazione di tipo rigid body (la stessa del capitolo 6):
T_k = [ R_{k−1,k} t_{k−1,k} ; 0 1 ]L'insieme T1:n = T1, …, Tn contiene tutte le trasformazioni in sequenza. Le posizioni della telecamera rispetto al frame iniziale (k = 0) si indicano con C0:n, e la posizione corrente si ottiene concatenando le trasformazioni:
C_n = C_{n−1} · T_ndove C0 può essere arbitrariamente specificata dall'utente. Lo scopo principale della VO è quindi calcolare le trasformazioni relative tra le immagini Ik e Ik−1 e concatenarle per ricostruire la traiettoria completa C0:n; per una stima più accurata della traiettoria locale è possibile raffinare iterativamente le ultime m pose.
Le trasformazioni rigid body T = [R t; 0 1] e la composizione di trasformazioni sono esattamente gli strumenti visti nel capitolo 6 per l'image stitching: lì servivano ad allineare immagini, qui a concatenare pose.
La pipeline del frontend si articola in quattro fasi: feature detection → feature matching/tracking → motion estimation → local optimization. Le condizioni necessarie per un funzionamento corretto:
Feature detection — l'obiettivo è individuare un insieme di feature fk al tempo k. In generale le feature salienti si trovano in aree dell'immagine ad alto contrasto; le tecniche sono basate su keypoint (capitolo 7).
Feature tracking vs feature matching — l'obiettivo di entrambi è trovare corrispondenze tra gli insiemi fk−1 e fk:
Il matching con descrittori locali è il tema del capitolo 7: la VO li usa per trovare le corrispondenze tra frame consecutivi, e le stesse corrispondenze alimentano la stima del movimento della sezione successiva.
L'obiettivo della motion estimation è calcolare la trasformazione Tk tra le due immagini Ik−1 e Ik. Esistono due categorie di approcci:
A seconda della rappresentazione delle feature fk−1 e fk si usano tecniche diverse:
Usa due insiemi di punti 3D, Xk−1 e Xk (coordinate 3D delle feature nei due istanti). Serve la triangolazione dei punti 3D (es. camera stereo). La stima richiede almeno 3 corrispondenze non collineari.
Pipeline: (1) due coppie stereo (Il,k−1, Ir,k−1) e (Il,k, Ir,k); (2) estrazione e matching tra Il,k−1 e Il,k; (3) triangolazione per ciascuna coppia stereo → Xk−1, Xk; (4) trasformazione che minimizza le distanze 3D-3D Σ_i ‖X̃_k,i − T_k X̃_{k−1,i}‖; (5) concatenazione Ck = Ck−1 Tk; (6) ripeti.
Usa un insieme di punti 3D Xk−1 (corrispondenti a fk−1) e la loro retro-proiezione 2D fk nell'immagine Ik. Richiede almeno 3 corrispondenze.
Inizializzazione: due frame Ik−2, Ik−1 → estrazione e matching → triangolazione Xk−1. Frame successivi: (1) acquisizione Ik; (2) estrazione e matching con Ik−1; (3) calcolo della posa della camera (PnP) dai match 3D-2D; (4) triangolazione delle nuove feature (Ik−1, Ik) → Xk; (5) ripeti.
Usa fk−1 e fk entrambe in coordinate 2D (solo immagini). La stima richiede almeno 5 corrispondenze e si basa sulla geometria epipolare (sezione 11): la soluzione è la trasformazione che minimizza l'errore di retro-proiezione dei punti triangolati in ciascuna immagine.
Pipeline: (1) frame Ik; (2) estrazione e matching con Ik−1; (3) calcolo della matrice essenziale; (4) scomposizione in Rk e tk e costruzione di Tk; (5) calcolo della scala relativa e riscalatura di tk; (6) concatenazione; (7) ripeti.
Nel caso monoculare la scala assoluta della traslazione non può essere stimata a partire dalle sole immagini 2D: è comunque possibile calcolare la scala relativa per trasformazioni successive. Una soluzione: triangolare i punti Xk−1 e Xk in due frame consecutivi (a meno di una scala ignota) e calcolare le distanze tra coppie di punti 3D. La scala relativa è il rapporto tra le distanze di una coppia di punti in Xk−1 e la corrispondente coppia in Xk:
r = ‖X_{k−1,i} − X_{k−1,j}‖ / ‖X_{k,i} − X_{k,j}‖Per una stima più robusta, la scala è calcolata come mediana del rapporto r su più coppie di punti. Il widget qui sotto mostra l'idea: la traiettoria stimata con la sola odometria 2D-2D accumula drift; attivando il loop closing (sezione 10) il sistema «riconosce» di essere tornato nella posizione iniziale e corregge la traiettoria.
La traiettoria reale è un percorso circolare (verde). La stima incrementale (cobalto) accumula un errore a ogni passo: aumenta il livello di rumore per vedere crescere il drift. Attiva il loop closing per osservare la correzione globale della traiettoria.
Gli errori introdotti nella stima del movimento tra coppie di frame successivi si accumulano nel tempo, generando uno scostamento (drift) tra la traiettoria stimata e quella reale: l'incertezza nella stima della posa Ck = Ck−1 Tk è sempre crescente. È perciò importante limitare questa incertezza per le singole trasformazioni. Due osservazioni chiave:
Σi,j ‖Ci − Ti,j Cj‖;Per la soluzione del problema si usa l'algoritmo Levenberg-Marquardt. In forma di errore di retro-proiezione, la soluzione minimizza
T_k = arg min_{X_i, C_k} Σ_{i,k} ‖ p_{ik} − g(X_i, C_k) ‖dove pik è l'i-esimo punto immagine del landmark 3D Xi misurato nella k-esima immagine, e g(Xi, Ck) è la sua retro-proiezione stimata sulla base della posa corrente Ck.
Loop closing (o loop closure detection) — ha l'obiettivo di verificare se l'oggetto in movimento è tornato in una posizione vista precedentemente. Se effettuato a partire dalle sole immagini, il problema si traduce in un problema di ricerca di similarità tra immagini. In caso di detection di un loop, l'informazione viene trasmessa al backend per aggiustare mappa e traiettoria (global bundle adjustment): se l'algoritmo di loop detection è affidabile, questo permette di eliminare il drift accumulato.
La ricerca di similarità tra immagini per il loop closing è il problema dei sistemi CBIR (capitolo 5) e del Bag of Words (capitolo 8): ORB-SLAM, ad esempio, usa un dizionario BoW proprio per riconoscere i luoghi già visitati.
Reconstruction — è il processo di costruzione di una mappa, ovvero di una descrizione dell'ambiente. La tipologia di mappa dipende strettamente dall'applicazione:
La stima 2D-2D si basa sui principi della geometria epipolare. Consideriamo due camere con centri ottici C e C′ che osservano lo stesso punto 3D X, proiettato in x sull'immagine di sinistra e x′ su quella di destra. I quattro punti C, C′, x, x′ e X sono complanari: tutti i punti del piano π saranno proiettati sulle rette l e l′ (le linee epipolari).
Per camere calibrate (parametri intrinseci noti), la relazione tra i punti corrispondenti è p′ = R·p + t. Manipolando l'equazione si ricava il vincolo epipolare:
E = [t]× · R (essential matrix)
p′ᵀ E p = 0 (epipolar constraint)con [t]× la matrice antisimmetrica associata al vettore t. La matrice essenziale E può essere calcolata a partire dalle coordinate dell'immagine usando SVD; sono necessarie almeno 5 corrispondenze (un numero superiore garantisce un risultato più accurato). Esiste anche una soluzione più semplice, quella di Longuet-Higgins, basata su 8 punti (SVD). La matrice essenziale può poi essere scomposta in R e t, sempre usando SVD.
Acronimo di Simultaneous Localization And Mapping: costruire una mappa, localizzando al tempo stesso un robot all'interno di quella mappa. Coinvolge due problemi complementari: mapping («come appare il mondo?») e localization («dove mi trovo?»), legati da una dipendenza circolare.
Nel pose tracking la posizione iniziale è nota e va raffinata nel tempo; nella global localization la posizione iniziale è ignota e va determinata da zero.
Perché per una localizzazione accurata è necessaria una buona mappa, ma la costruzione di una buona mappa richiede una stima accurata della posa: i due problemi si richiedono a vicenda, ed è per questo che lo SLAM li risolve simultaneamente.
Monoculari: nessuna informazione di distanza da una singola immagine, serve muovere la camera, solo valori relativi con scala ignota. Stereoscopiche: telecamere sincronizzate a distanza prefissata (baseline), posizione 3D di ogni pixel calcolabile, elevata complessità. RGB-D: luce strutturata o Time-of-Flight con ricostruzione 3D operata dal sensore; range limitato, rumore e FoV ridotto (indoor).
1) Acquisizione e pre-processing dei dati del sensore; 2) Visual Odometry (frontend) — stima del movimento e mappa locale approssimata; 3) Backend (filters/graphs) — ottimizzazione di mappa e traiettoria; 4) Loop closing — verifica del ritorno in posizioni viste (riduzione drift); 5) Reconstruction — costruzione della mappa per il task.
Motion: x_k = f(x_{k−1}, u_k, w_k) con uk comando in input e wk rumore. Observation: z_{k,j} = h(y_j, x_k, v_{k,j}) con vk,j rumore del camera model. Lo spostamento è descritto da motion (valore di x) e observation (rilevamento di un landmark yj in posizione xk).
Nel caso lineare con rumore Gaussiano delle motion/observation equation. I casi non lineari o con rumore non Gaussiano richiedono tecniche di ottimizzazione non lineare o approcci più complessi dei Kalman Filters.
La VO non è influenzata dallo slittamento delle ruote su terreni irregolari o in condizioni avverse; la stima della traiettoria è più accurata; può essere combinata con wheel odometry, GPS, IMU e laser odometry; è fondamentale dove il GPS non è disponibile (ambienti subacquei, aerei, indoor).
Concatenando le trasformazioni relative tra frame consecutivi: C_n = C_{n−1} · T_n, dove Tk è la trasformazione rigid body [R_{k−1,k} t_{k−1,k}; 0 1] e C0 è arbitraria. La VO ricostruisce quindi il percorso in modo incrementale.
Il tracking fa una ricerca locale di ciascuna feature (es. predizione con filtri e correlazione); il matching fa una ricerca indipendente delle feature in ciascuna immagine e poi cerca le corrispondenze sulla base di descrittori locali (es. ORB, SIFT, SURF).
Ambiente sufficientemente illuminato; predominanza di elementi statici rispetto a quelli in movimento; presenza di sufficiente tessitura per stimare il movimento apparente; sufficiente sovrapposizione della scena tra frame consecutivi.
3D-3D: due insiemi di punti 3D (triangolazione, es. stereo); almeno 3 corrispondenze non collineari; minimizza la distanza 3D-3D. 3D-2D: punti 3D di Xk−1 e retro-proiezione 2D in Ik; almeno 3 corrispondenze; risolto con PnP. 2D-2D: soli punti 2D; almeno 5 corrispondenze; basato sulla geometria epipolare (matrice essenziale).
La scala assoluta della traslazione non può essere stimata dalle sole immagini 2D. Si triangolano i punti Xk−1 e Xk in due frame consecutivi e la scala relativa è r = ‖X_{k−1,i} − X_{k−1,j}‖ / ‖X_{k,i} − X_{k,j}‖, calcolata come mediana su più coppie per robustezza.
Un'ottimizzazione locale che usa, oltre alle trasformazioni tra frame adiacenti, vincoli tra frame non adiacenti (Ti,j), limitando l'analisi agli m frame precedenti per efficienza. Minimizza Σ_{i,j} ‖C_i − T_{i,j} C_j‖ (o l'errore di retro-proiezione) con l'algoritmo Levenberg-Marquardt, limitando l'accumulo di drift.
Verifica se l'oggetto è tornato in una posizione vista precedentemente; con sole immagini è un problema di ricerca di similarità tra immagini. Se viene rilevato un loop, l'informazione è trasmessa al backend che esegue un global bundle adjustment, aggiustando mappa e traiettoria ed eliminando il drift accumulato (se la detection è affidabile).
Le metrical maps descrivono l'esatta posizione degli oggetti (sparse per la localizzazione, dense per la navigazione; 2D/3D, nuvole di punti, mesh con texture). Le topological maps descrivono le relazioni spaziali tra gli elementi: sono grafi di nodi e archi.
La baseline (segmento tra i centri ottici C e C′) interseca i piani immagine in due punti detti epipoli (e, e′). Ogni piano che include la baseline è un piano epipolare; la sua intersezione con il piano immagine è la linea epipolare. Il punto 3D X proiettato in x e x′ giace su un piano epipolare: x′ deve stare sulla linea epipolare l′ associata a x.
Per camere calibrate, E = [t]× · R (con [t]× matrice antisimmetrica) e il vincolo epipolare è p′ᵀ E p = 0. E si calcola dalle coordinate dell'immagine con SVD servono almeno 5 corrispondenze (più corrispondenze → più accuratezza); esiste la soluzione di Longuet-Higgins basata su 8 punti. E si scompone poi in R e t, sempre con SVD.