Odometria Visuale e Bundle Adjustment

La Visual Odometry si pone come obiettivo quello di ricavare la posa relativa che ha assunto una camera (o una coppia stereoscopica) che si muove nello spazio analizzando due immagini in sequenza. Il problema dell'odometria visuale per una sola telecamera si risolve normalmente con il calcolo della matrice essenziale e la sua successiva decomposizione. In questo caso, come già indicato in precedenza, non è possibile conoscere la scala del movimento, ma solo mettere in relazione tra loro i vari movimenti. Discorso differente nel caso in cui si ha a disposizione una coppia stereoscopica.

Data una serie di osservazioni temporali di punti mondo ricavati dalla ricostruzione tridimensionale $(\mathbf{x}_i,\mathbf{x}'_i)$ è possibile ricavare in maniera lineare una trasformazione di rototraslazione $(\mathbf{R}, \mathbf{t})$ che trasforma i punti del mondo all'istante di tempo $t$ all'istante di tempo $t'$ in modo da poter essere espressi con una equazione del tipo:

$$\mathbf{x}'_i = \mathbf{R} \mathbf{x}_i + \mathbf{t}$$ (9.87)

Tale approccio è generale e non dipende dal particolare sensore utilizzato per ricavare i punti.

La rototraslazione eseguita dalla coppia di sensori può essere ricavata minimizzando la quantità:

$$\sum_i \Vert \mathbf{x}'_i - \mathbf{R} \mathbf{x}_i - \mathbf{t} \Vert^2$$ (9.88)

La soluzione a 12 parametri, lineare da dati sovradimensionati, troverà un minimo assoluto ma non è lo stimatore ottimo, in quanto minimizza una quantità algebrica e in ogni caso non garantisce che la matrice di rotazione sia ortonormale. Partendo dalla soluzione lineare, l'utilizzo di un minimizzatore non-lineare (per esempio Levenberg-Marquardt, sezione 3.3.5) sulla funzione costo di equazione (9.88) permette di determinare i 6 parametri (3 rotazioni e 3 traslazioni) in modo più preciso. Questa è algoritmo è indicato come 3D-to-3D perchxE9 ricava il movimento partendo da coppie di punti tridimensionali. Come alternativa alla soluzione lineare è possibile anche una soluzione in forma chiusa (Hor87).

L'approccio mostrato ora è generale ma mal si adatta al caso di punti mondo ottenuti da una ricostruzione tridimensionale da immagini. La funzione costo mostrata, infatti, ottimizza quantità in coordinate mondo e non in coordinate immagine: il rumore sui punti dell'immagine si propaga in maniera non lineare durante la fase di triangolazione e perciò solo in coordinate immagine è possibile supporre che il rumore di individuazione dei punti sia gaussiano a media nulla. Non è pertanto possibile realizzare uno stimatore a massima verosimiglianza sfruttando solamente i punti in coordinate mondo. Un approccio più raffinato è quello indicato come 3D-to-2D dove si cerca di minimizzare la riproiezione di un punto del passato in coordinate immagine:

$$\sum_i \Vert \mathbf{p}_i - \hat{\mathbf{p}_i} \Vert^2$$ (9.89)

dove $\hat{\mathbf{p}_1}$ è la proiezione, rototraslata, del punto tridimensionale $\mathbf{x}_i$ ottenuto dal fotogramma precedente. Questo problema è anche conosciuto come perspective from n points (PnP) in quanto molto simile al problema già visto in precedenza della calibrazione di una camera in ambiente statico.

Chiaramente anche questo approccio è inficiato dal fatto che il punto tridimensionale $\mathbf{x}_i$ non è un dato del problema ma è conosciuto con una certa quantità di errore. Per questa ragione è necessario fare un ulteriore passo minimizzando entrambi gli errori in coordinate immagine (è la Maximum Likelihood Estimation):

$$\sum_i \Vert \mathbf{p}_1 - \hat{\mathbf{p}_1} \Vert^2 + \Ve... ..._1 \Vert^2 + \Vert \mathbf{p}'_2 - \hat{\mathbf{p}}'_2 \Vert^2$$ (9.90)

avendo imposto $\hat{\mathbf{p}_1} = \mathbf{K}_1\mathbf{R}_1 (\hat{\mathbf{x}}_i - \mathbf{t}_1)$, $\hat{\mathbf{p}_2} = \mathbf{K}_2\mathbf{R}_2 (\hat{\mathbf{x}}_i - \mathbf{t}_2)$, $\hat{\mathbf{p}'_1} = \mathbf{K}_1\mathbf{R}_1 (\hat{\mathbf{x}'}_i - \mathbf{t}_1)$ e $\hat{\mathbf{p}'_2} = \mathbf{K}_2\mathbf{R}_2 (\hat{\mathbf{x}'}_i - \mathbf{t}_2)$ a cui va aggiunto il vincolo di equazione (9.87), mantenendo l'incognita sull'effettiva posizione del punto $\hat{\mathbf{x}}_i$ nei due sistemi di riferimento. In questo modo viene sia minimizzato lo spostamento che eseguono le camere, sia la coordinata tridimensionale di ogni singola feature nel mondo. Anche in questo caso la soluzione alla massima verosimiglianza richiede di risolvere un problema non lineare di dimensioni notevoli. Nel caso di una coppia stereo rettificata, la funzione costo può essere di molto semplificata.

L'odometria visuale è un algoritmo di dead-reckoning e pertanto è affetto da deriva. È possibile estendere questi ragionamenti al caso in cui non siano solo due gli istanti di tempo coinvolti nella minimizzazione ma molteplici. In questo caso si entra in un discorso complicato per cercare di ridurre il più possibile gli errori di deriva nel comporre le diverse trasformazioni. Un tutorial che affronta queste tematiche è (SF11).

Quando si vuole affrontare il problema dal punto di vista bayesiano, sfruttando l'equazione (9.90), e si intendono processare contemporaneamente tutti i fotogrammi, invece che odometria visuale si preferisce parlare di Bundle Adjustment.

Il concetto di Bundle Adjustment, introdotto dalla fotogrammetria e poi acquisito dalla Computer Vision (si veda l'ottimo survey (TMHF00)), indica una minimizzazione multivariabile in modo da ottenere contemporaneamente una ricostruzione tridimensionale, le pose relative della camere in una sequenza di immagini ed eventualmente i parametri intrinseci delle camere stesse.

Si tratta di una estensione alle tecniche non-lineari che stimano i parametri attraverso la minimizzazione di una funzione di costo adeguata basata sugli errori di riproiezione dei punti individuati, nella stessa forma di equazione (9.90).

Siccome la stessa feature può essere vista da diverse immagini, il processo di stima condiziona tutte le pose e di conseguenza il problema non si può scomporre in $n$ problemi separati di odometria visuale: tutte le immagini della sequenza devono essere minimizzate contemporaneamente. Per questo motivo il problema della Bundle Adjustment è un problema dimensionalmente elevato, sicuramente non-convesso, che richiede una ottimizzazione non semplice e fa ricorso a minimizzazione sparsa per preservare memoria e migliorare la precisione.

Un approccio alternativo al Bundle Adjustment, sicuramente non il miglior stimatore alla massima verosimiglianza ma che introduce un numero minore di incognite, è quello del Pose Graph Optimization (GKSB10) che, sfruttando informazione della medesima posa ottenuta da più percorsi ovvero avendo individuato dei Loop, permette di ottimizzare solamente le pose rispetto a quelle ottenute dall'odometria visuale. Sia $\mathbf{x} = \left( \mathbf{x}_1, \ldots, \mathbf{x}_n \right)$ un vettore di parametri dove l'elemento $\mathbf{x}_i$ rappresenta la posa del nodo i-esimo. Siano $\mathbf{z}_{ij}$ e $\boldsymbol\Omega_{ij}$ la misura e la matrice di precisione dell'osservazione virtuale della posa relativa tra i nodi i e j. L'obiettivo è ottenere una stima dei parametri $\mathbf {x}$ date le osservazioni virtuali $\mathbf{z}_{ij}$. Siccome le pose relative vengono ottenute come confronto di due pose assolute, parametri da ottenere, si può definire la funzione costo

$$\mathbf{e}_{ij} = \mathbf{e}_{ij}\left( \mathbf{x}_i, \math... ...hat{\mathbf{z}}_{ij} \left( \mathbf{x}_i, \mathbf{x}_j \right)$$ (9.91)

misura dell'errore tra la posa relativa virtuale misurata $\mathbf{z}_{ij}$ e quella predetta $\hat{\mathbf{z}}_{ij} \left( \mathbf{x}_i, \mathbf{x}_j \right)$ date le configurazioni $\mathbf{x}_i$ e $\mathbf{x}_j$ da valutare rispettivamente per i nodi i e j. Sfruttando l'informazione sulla precisione della stima della singola posa relativa è possibile definire una funzione costo globale

$$F(\mathbf{x}) = \sum_{< i,j >} \mathbf{e}^{\top}_{ij} \boldsymbol\Omega_{ij} \mathbf{e}_{ij}$$ (9.92)

di fatto somma delle singole distanze di Mahalanobis tra tutte le coppie $(i,j)$ su cui è stata fatta una misura di posa relativa. La funzione $F(\mathbf{x})$, minimizzata rispetto a $\mathbf {x}$, fornisce la miglior stima delle pose assolute del problema, tutto senza coinvolgere i singoli elementi di cui è composta la singola osservazione reale.