11. V-SLAM

V-SLAM — Simultaneous Localization and Mapping

Slide

Indice

• Definizione di SLAM
  • Mapping vs Localization
  • SLAM e Visual SLAM
• Framework Visual SLAM — Panoramica
• Formulazione Matematica dello SLAM
  • Motion Equation e Observation Equation
  • State Estimation Problem
• Visual Odometry (VO)
  • Formulazione del Problema
  • Pipeline della Visual Odometry
• Step 1 — Feature Detection
• Step 2 — Feature Matching e Tracking
• Step 3 — Motion Estimation
  • 3D-3D
  • 3D-2D
  • 2D-2D e Scala Relativa
• Step 4 — Local Optimization (Bundle Adjustment)
• Backend Optimization e Loop Closing
• Reconstruction
• Appendice — Geometria Epipolare

---

Definizione di SLAM

Mapping vs Localization

Definizione

SLAM (Simultaneous Localization and Mapping) è il problema di costruire una mappa di un ambiente sconosciuto mentre si stima simultaneamente la posizione di un agente (robot, veicolo, drone) all’interno di quella mappa.

I due problemi sono intimamente legati:

• Mapping (Come appare il mondo?): integrazione delle informazioni dei sensori in una rappresentazione dell’ambiente
• Localization (Dove mi trovo?): stima della posizione del robot nella mappa

Il circolo vizioso: per localizzarsi accuratamente serve una buona mappa, ma per costruire una buona mappa serve una stima accurata della posizione.

Varianti della localizzazione:

• Pose tracking: posizione iniziale nota, si traccia il movimento
• Global localization: posizione iniziale ignota, più difficile

SLAM e Visual SLAM

Lo SLAM può essere realizzato con sensori di varia natura: IMU, sonar, infrarossi, laser (LIDAR).

Definizione

Il Visual SLAM (V-SLAM) utilizza esclusivamente sensori visivi (camere monoculari, stereoscopiche, RGB-D) per risolvere il problema SLAM.

Vantaggi del V-SLAM:

• L’informazione visiva è molto più ricca rispetto ad altri sensori
• Sensori di costo limitato (webcam, smartphone)

Svantaggio: maggiore complessità computazionale, difficile da usare in tempo reale

---

Framework Visual SLAM — Panoramica

Il tipico framework V-SLAM si articola in 5 componenti:

Sensor data → [Frontend: Visual Odometry] → [Backend: Filters/Graphs] → Reconstruction
                          ↓                          ↑
                    [Loop Closing] ─────────────────┘

#	Componente	Scopo
1	Sensor data	Acquisizione e pre-processing delle immagini
2	Visual Odometry (Frontend)	Stima del movimento della camera e generazione di una mappa locale approssimata
3	Backend (Filters/Graphs)	Ottimizzazione per generare mappa e traiettoria accurate
4	Loop Closing	Verifica del passaggio in posizioni precedentemente viste (riduzione drift)
5	Reconstruction	Costruzione della mappa specifica per il task

---

Formulazione Matematica dello SLAM

Motion Equation e Observation Equation

Il sistema acquisisce dati a intervalli discreti. Si definiscono:

• ${\mathbf{x}}_1,\dots ,{\mathbf{x}}_k$: sequenza di pose (posizioni) che rappresenta la traiettoria
• ${\mathbf{y}}_1,\dots ,{\mathbf{y}}_N$: insieme di $N$ landmark della mappa

Il sistema è descritto da due equazioni:

Motion equation — come evolve la posa: ${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k)$

dove ${\mathbf{u}}_k$ è il comando di controllo in input e ${\mathbf{w}}_k$ è il rumore.

Observation equation — cosa osserva la camera: ${\mathbf{z}}_{k,j}=h({\mathbf{y}}_j,{\mathbf{x}}_k,{\mathbf{v}}_{k,j})$

dove ${\mathbf{z}}_{k,j}$ è l’osservazione del landmark $j$ dalla posizione $k$, e ${\mathbf{v}}_{k,j}$ è il rumore (modellato dal modello di camera).

Esempio di motion equation per un robot planare: ${\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_k={\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_{k-1}+{\left[\begin{array}{c}\Delta x_1\\ \Delta x_2\\ \Delta \theta \end{array}\right]}_k+{\mathbf{w}}_k$

State Estimation Problem

Il problema centrale

State Estimation Problem: come stimare $\mathbf{x}$ (localizzazione) e $\mathbf{y}$ (mapping), dato un input di controllo $\mathbf{u}$ e un’osservazione $\mathbf{z}$?

La soluzione dipende da:

• Forma delle equazioni (lineare / non lineare)
• Distribuzione del rumore (gaussiano / non gaussiano)

Il caso più semplice — lineare con rumore gaussiano — ha soluzione ottimale con i Filtri di Kalman (KF). Casi più complessi richiedono ottimizzazione non lineare.

---

Visual Odometry (VO)

Definizione

La Visual Odometry è 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.

Vantaggio rispetto alla wheel odometry: la VO non è influenzata dallo slittamento delle ruote su terreni irregolari. Può essere combinata con wheel odometry, GPS, IMU, laser. È fondamentale in ambienti senza GPS (subacqueo, spazio).

Limite: la sola VO causa l’accumulo di un drift — uno scostamento crescente tra traiettoria stimata e reale, dovuto alla propagazione degli errori di stima.

Formulazione del Problema

Un agente acquisisce immagini a intervalli discreti $k$. Le sequenze di immagini per i diversi tipi di camera:

• Monoculare: $I_{0:n}=\{I_0,\dots ,I_n\}$
• Stereo: $I_{l,0:n}$ e $I_{r,0:n}$ (camera sinistra e destra)
• RGB-D: $I_{0:n}$ (colore) e $D_{0:n}$ (profondità)

Le posizioni di camera consecutive $k-1$ e $k$ sono collegate da una trasformazione rigid body:

$T_k=\left[\begin{array}{cc}{R}_{k-1,k}& t_{k-1,k}\\ 0& 1\end{array}\right]$

La posizione corrente si ottiene per concatenazione: $C_n=C_{n-1}{T}_n$

Lo scopo principale della VO è calcolare le trasformazioni relative $T_k$ tra frame consecutivi e concatenarle per ricostruire la traiettoria completa $C_{0:n}$.

Pipeline della Visual Odometry

Feature Detection → Feature Matching/Tracking → Motion Estimation → Local Optimization

Condizioni necessarie per il funzionamento:

• Ambiente sufficientemente illuminato
• Predominanza di elementi statici nella scena
• Sufficiente tessitura che consenta la stima del movimento apparente
• Sufficiente sovrapposizione della scena tra frame consecutivi

---

Step 1 — Feature Detection

Obiettivo: individuare un insieme di feature $f_k$ al tempo $k$.

Le feature salienti si trovano in aree ad alto contrasto dell’immagine. Si usano tecniche basate su keypoint: Harris, FAST, SIFT, ORB….

---

Step 2 — Feature Matching e Tracking

Obiettivo: trovare corrispondenze tra i due insiemi di feature $f_{k-1}$ e $f_k$.

Due approcci:

Approccio	Descrizione
Feature tracking	Ricerca locale di ciascuna feature nel frame successivo (predizione con filtri + correlazione). Più veloce, adatto a frame consecutivi vicini
Feature matching	Ricerca indipendente delle feature in ciascuna immagine, poi corrispondenza per descrittori locali (ORB, SIFT, SURF…). Più robusto a grandi spostamenti

---

Step 3 — Motion Estimation

Obiettivo: calcolare la trasformazione $T_k$ tra $I_{k-1}$ e $I_k$.

Approcci:

• Appearance-based: analisi dei valori di luminosità di tutti i pixel. Meno accurato, più oneroso computazionalmente.
• Feature-based (usato in VO): usa feature salienti e ripetibili. Più robusto ed efficiente.

A seconda del tipo di sensore, le corrispondenze possono essere rappresentate in modo diverso, portando a tre varianti:

Tipo	Descrizione	Sensore
3D-3D	Entrambi gli insiemi in coordinate 3D	Stereo
3D-2D	$f_{k-1}$ in 3D, $f_k$ in 2D	Mono, Stereo
2D-2D	Entrambi in coordinate 2D	Mono, Stereo

3D-3D

Richiede la triangolazione dei punti 3D (es. camera stereo) — almeno 3 corrispondenze non collineari.

Algoritmo (caso stereo):

1. Acquisire due coppie stereo $I_{l,k-1},I_{r,k-1}$ e $I_{l,k},I_{r,k}$
2. Estrazione e matching delle feature tra $I_{l,k-1}$ e $I_{l,k}$
3. Triangolazione delle feature: $(I_{l,k-1},I_{r,k-1})\Rightarrow X_{k-1}$ e $(I_{l,k},I_{r,k})\Rightarrow X_k$
4. Trovare la trasformazione che minimizza le distanze 3D: $T_k=\left[\begin{array}{cc}{R}_{k,k-1}& t_{k,k-1}\\ 0& 1\end{array}\right]=\underset{T_k}{\mathrm{argmin}}{\sum }_i\Vert {\tilde{X}}_k^i-T_k{\tilde{X}}_{k-1}^i\Vert$
5. Concatenazione: $C_k=C_{k-1}{T}_k$
6. Ripeti da 1

3D-2D

$f_{k-1}$ è rappresentato in 3D, $f_k$ in 2D. Richiede almeno 3 corrispondenze. Usato anche con camera monoculare.

Inizializzazione (necessaria per ottenere il primo $X_{k-1}$):

• Si acquisiscono due frame $I_{k-2},I_{k-1}$
• Si estraggono e si matchano le feature
• Si triangolano: $I_{k-2},I_{k-1}\Rightarrow X_{k-1}$

Frame successivi:

1. Acquisire il frame $I_k$
2. Estrazione delle feature e matching con $I_{k-1}$
3. Calcolo della posa della camera (PnP — Perspective-n-Point) a partire dai match 3D-2D tra $X_{k-1}$ e $f_k$
4. Triangolazione delle nuove feature: $I_{k-1},I_k\Rightarrow X_k$
5. Ripeti da 1

2D-2D e Scala Relativa

Entrambi gli insiemi sono in 2D. Richiede almeno 5 corrispondenze. La soluzione minimizza l’errore di retro-proiezione dei punti triangolati e si basa sulla geometria epipolare (vedi appendice).

Algoritmo:

1. Acquisire il frame $I_k$
2. Estrazione feature e matching con $I_{k-1}$
3. Calcolo della matrice essenziale per la coppia $I_{k-1},I_k$
4. Scomposizione della matrice essenziale in $R_k$ e $t_k$ (SVD) → costruzione di $T_k$
5. Calcolo della scala relativa e riscalatura di $t_k$
6. Concatenazione: $C_k=C_{k-1}{T}_k$
7. Ripeti da 1

Problema della scala

Con una camera monoculare la scala assoluta della traslazione non può essere stimata dalle sole immagini 2D. È possibile calcolare solo la scala relativa tra trasformazioni successive, triangolando i punti $X_{k-1}$ e $X_k$: $r=\frac{\Vert X_{k-1,i}-X_{k-1,j}\Vert }{\Vert X_{k,i}-X_{k,j}\Vert }$ Per robustezza, la scala si stima come la mediana di $r$ calcolata su più coppie di punti.

---

Step 4 — Local Optimization (Bundle Adjustment)

Gli errori di stima del movimento tra coppie di frame consecutive si accumulano nel tempo, generando un drift crescente. L’incertezza sulla posa $C_k=C_{k-1}{T}_k$ è sempre crescente.

Windowed Bundle Adjustment: per limitare il drift, si ottimizzano le stime sulle ultime $m$ pose simultaneamente, usando anche trasformazioni tra frame non adiacenti $T_{i,j}$ come vincoli aggiuntivi.

La funzione obiettivo minimizza l’errore di retro-proiezione:

$T_k=\left[\begin{array}{cc}{R}_{k-1,k}& t_{k-1,k}\\ 0& 1\end{array}\right]=\underset{X_i,C_k}{\mathrm{argmin}}{\sum }_{i,k}\Vert p_i^k-g(X_i,C_k)\Vert$

dove $p_i^k$ è il punto immagine del landmark $X_i$ misurato nella $k$-esima immagine, e $g(X_i,C_k)$ è la sua retro-proiezione stimata. Si risolve con Levenberg-Marquardt.

---

Backend Optimization e Loop Closing

Backend: gestisce il rumore presente nel sistema. A partire dai dati rumorosi, stima lo stato completo — traiettoria e mappa. Richiede modelli per l’incertezza (filtri e tecniche di ottimizzazione).

Loop Closing (loop closure detection): verifica se l’agente è tornato in una posizione già visitata. Dal punto di vista visivo è un problema di ricerca di similarità tra immagini — si può usare BoW per trovare immagini simili in modo efficiente.

Se viene rilevato un loop, l’informazione viene trasmessa al backend che aggiusta la mappa e la traiettoria (global bundle adjustment), eliminando il drift accumulato.

---

Reconstruction

Obiettivo: costruire una descrizione dell’ambiente (mappa).

La tipologia di mappa dipende dall’applicazione (2D/3D, nuvola di punti, mesh, superficie con texture).

Tipo	Descrizione
Metrical maps	Descrivono l’esatta posizione degli oggetti. Possono essere sparse (per localizzazione) o dense (per navigazione)
Topological maps	Descrivono le relazioni spaziali tra elementi. Sono grafi con nodi (luoghi) e archi (connessioni)

---

Appendice — Geometria Epipolare

La geometria epipolare descrive la relazione geometrica tra due viste della stessa scena.

Concetti chiave:

• I centri ottici $C,C^{\prime }$, i punti immagine $x,x^{\prime }$ e il punto 3D $X$ sono complanari — il piano che li contiene è il piano epipolare $\pi$
• La baseline (segmento $C{C}^{\prime }$) interseca i due piani immagine in due punti $e,e^{\prime }$ detti epipoli
• L’intersezione di un piano epipolare con il piano immagine è la linea epipolare

Camere convergenti: gli epipoli sono all’interno delle immagini, le linee epipolari convergono verso di essi.

Camere parallele: gli epipoli sono all’infinito, le linee epipolari sono parallele (caso tipico delle camere stereo standard).

Matrice Essenziale

Per camere calibrate (coordinate normalizzate), la relazione tra due viste è espressa dalla matrice essenziale $E$:

$E=[t{]}_{\times }R\text{con}[t{]}_{\times }=\left[\begin{array}{ccc}0& -t_z& t_y\\ t_z& 0& -t_x\\ -t_y& t_x& 0\end{array}\right]$

Il vincolo epipolare è:

${\mathbf{p}}^{\prime }E\mathbf{p}=0$

Come si usa in pratica:

• $E$ può essere calcolata dalle coordinate immagine tramite SVD
• Richiede almeno 5 corrispondenze (metodo di Nister); esiste anche la soluzione a 8 punti di Longuet-Higgins (SVD)
• $E$ si decompone in $R$ e $t$ (sempre con SVD)
• La decomposizione produce 4 soluzioni possibili; si seleziona quella con punti davanti a entrambe le camere

---

Prossimi Argomenti

Continueremo con:

• Deep Learning per Classificazione - CNN, AlexNet, VGG, ResNet
• Deep Learning per Object Detection - YOLO, Faster R-CNN