Pose Graph Optimization

Un approccio alternativo al Bundle Adjustment, che introduce un numero minore di variabili, è il Pose Graph Optimization  (GKSB10). In questo caso i punti tridimensionali non vengono inclusi esplicitamente nelle variabili del problema e si ottimizzano solamente le pose dei diversi fotogrammi o nodi della sequenza.

Il vantaggio di questa formulazione è particolarmente evidente quando sono disponibili osservazioni relative tra pose non consecutive, ad esempio in presenza di un loop closure. Una stessa configurazione della scena può infatti essere osservata nuovamente dopo un certo numero di fotogrammi, fornendo un vincolo relativo aggiuntivo che può essere utilizzato per ridurre la deriva accumulata dall'odometria.

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 inoltre $\mathbf{z}_{ij}$ e $\boldsymbol\Omega_{ij}$ rispettivamente la misura e la matrice di precisione dell'osservazione relativa tra i nodi $i$ e $j$.

L'obiettivo è ottenere una stima dei parametri $\mathbf {x}$ a partire dalle osservazioni relative $\mathbf{z}_{ij}$. Poiché la posa relativa tra due nodi può essere ottenuta confrontando le rispettive pose assolute, si può definire l'errore

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

dove $\hat{\mathbf{z}}_{ij}$ rappresenta la posa relativa prevista dalle configurazioni $\mathbf{x}_i$ e $\mathbf{x}_j$ dei due nodi.

Sfruttando l'informazione sulla precisione della singola osservazione relativa, è possibile definire una funzione costo globale

$$F(\mathbf{x}) = \sum_{\langle i,j\rangle} \mathbf{e}_{ij}^{\top} \boldsymbol\Omega_{ij} \mathbf{e}_{ij}.$$ (10.107)

La funzione $F(\mathbf{x})$ è quindi la somma delle distanze di Mahalanobis associate alle osservazioni relative disponibili tra le coppie di nodi.

La minimizzazione di $F(\mathbf{x})$ fornisce una stima delle pose assolute compatibile con le osservazioni relative e con le rispettive matrici di precisione, senza dover introdurre esplicitamente nel problema i singoli punti tridimensionali che hanno generato tali osservazioni.