Factor Graph e Smoothing

I problemi di Bundle Adjustment e Pose Graph Optimization introdotti nelle sezioni precedenti possono essere interpretati come particolari problemi di stima MAP (Maximum A Posteriori).

Nel Bundle Adjustment le variabili incognite comprendono tipicamente pose delle camere e punti tridimensionali osservati; nel Pose Graph Optimization vengono invece ottimizzate solamente le pose e i vincoli relativi tra esse. Entrambi i problemi possono essere rappresentati in modo unificato mediante il formalismo dei factor graph.

In molti problemi di visione artificiale e robotica risulta conveniente mantenere simultaneamente nel problema tutte le variabili incognite e tutte le osservazioni disponibili, invece di stimare ricorsivamente il solo stato corrente. Questa formulazione prende il nome di smoothing ed è alla base di numerosi algoritmi moderni di localizzazione, mappatura e fusione sensoriale.

I factor graph costituiscono una rappresentazione del problema di stima, mentre lo smoothing è una strategia di inferenza nella quale tutte le variabili del grafo vengono ottimizzate simultaneamente anziché marginalizzate progressivamente come nei filtri ricorsivi.

Dal punto di vista probabilistico si desidera determinare la configurazione più probabile delle variabili

$$\mathbf{x} = \{\mathbf{x}_1,\mathbf{x}_2,\ldots,\mathbf{x}_n\}$$

data una collezione di osservazioni $\mathbf{z}$:

$$\hat{\mathbf{x}} = \arg\max_{\mathbf{x}} p(\mathbf{x}\mid\mathbf{z}).$$

Le variabili possono rappresentare pose, landmark, parametri di calibrazione o, più in generale, qualunque quantità incognita del problema.

Nei problemi sequenziali la distribuzione a posteriori può essere fattorizzata come

$$p(\mathbf{x}_{0:k}\mid\mathbf{z}_{1:k}) \propto p(\mathbf{x}... ...hbf{x}_{i-1}) \prod_{i=1}^{k} p(\mathbf{z}_i\mid\mathbf{x}_i).$$

Questa fattorizzazione può essere rappresentata mediante un factor graph, ovvero un grafo bipartito costituito da nodi variabile e nodi fattore. I nodi variabile rappresentano le quantità da stimare, mentre i nodi fattore rappresentano i vincoli probabilistici associati alle osservazioni e alle informazioni a priori.

Figura 10.4: Esempio di factor graph per un problema di stima. I nodi circolari rappresentano le variabili del problema mentre i nodi quadrati rappresentano i fattori associati alle osservazioni e ai vincoli.

minimizzazione di una somma di residui pesati:

Assumendo errori gaussiani indipendenti, la stima di massima verosimiglianza (MLE), e più in generale la stima MAP in presenza di fattori a priori, si riconduce alla minimizzazione di una somma di residui pesati:

$$\hat{\mathbf{x}} = \arg\min_{\mathbf{x}} \sum_i \mathbf{e}_i... ...thbf{x})^\top \mathbf{\Sigma}_i^{-1} \mathbf{e}_i(\mathbf{x}),$$

dove $\mathbf{e}_i(\mathbf{x})$ rappresenta il residuo associato al fattore $i$ e $\mathbf{\Sigma}_i$ la relativa covarianza.

I factor graph costituiscono quindi un formalismo unificante nel quale possono essere espressi Bundle Adjustment, Pose Graph Optimization, Visual-Inertial Odometry, SLAM e più in generale problemi di fusione sensoriale.

A differenza dei filtri ricorsivi, che marginalizzano continuamente gli stati passati, le tecniche di smoothing mantengono esplicitamente un insieme di variabili e consentono alle nuove osservazioni di correggere anche stime già effettuate.

Nel caso di errori gaussiani, l'inferenza su factor graph si riduce infine alla risoluzione di un problema di ottimizzazione non lineare ai minimi quadrati.