Factor Graph

Categoria: State estimation & navigation › Localisation
Tag: state-estimation, slam, probabilistic-inference, optimization, sensor-fusion

Un grafo bipartito che rappresenta una distribuzione di probabilità come prodotto di fattori, collegando le variabili incognite, come pose e punti di riferimento, alle misure e alle distribuzioni a priori che le vincolano.

Definizione

Un grafo dei fattori (factor graph) ha due tipi di nodi. I nodi variabile rappresentano le incognite da stimare, come pose del robot, velocità, bias dei sensori o posizioni dei punti di riferimento. I nodi fattore rappresentano informazioni su di esse, come una distribuzione a priori, una misura di odometria tra due pose o l'osservazione di un punto di riferimento da parte di una telecamera, e ogni fattore è collegato solo alle variabili che coinvolge. Il grafo descrive una distribuzione congiunta pari al prodotto di tutti i suoi fattori, il che rende esplicita la struttura di un problema: quali incognite sono collegate e quali misure sono vincoli sparsi e locali.

I grafi dei fattori sono stati formulati come quadro generale per l'inferenza da Kschischang, Frey e Loeliger (2001) e sono stati ampiamente adottati in robotica, in gran parte grazie al lavoro di Dellaert e Kaess su smoothing e mapping. Con modelli di rumore gaussiani, trovare i valori più probabili di tutte le variabili dati tutti i fattori è un problema ai minimi quadrati non lineari: ogni fattore contribuisce con un residuo pesato dalla sua matrice di informazione. I solutori linearizzano i residui, sfruttano la sparsità del grafo per fattorizzare in modo efficiente il sistema lineare risultante e iterano con Gauss-Newton o Levenberg-Marquardt. Solutori incrementali come iSAM2 aggiornano la soluzione man mano che arrivano nuovi fattori senza risolvere tutto da capo. GTSAM implementa direttamente i grafi dei fattori, e librerie generali per i minimi quadrati come g2o e Ceres vengono usate per lo stesso tipo di problemi.

Il principale vantaggio pratico è la modularità. Un nuovo sensore diventa un nuovo tipo di fattore con il proprio residuo e il proprio modello di rumore, così la preintegrazione dell'IMU, il GPS, l'odometria delle ruote, gli allineamenti delle scansioni LiDAR, le chiusure del loop e i modelli di misura appresi possono convivere in un unico stimatore. L'ottimizzazione del grafo delle pose è un caso particolare in cui ogni variabile è una posa e ogni fattore un vincolo di posa relativa; un grafo dei fattori generale può contenere anche punti di riferimento, velocità, bias e parametri di calibrazione. Gli smoother a ritardo fisso marginalizzano le variabili vecchie per limitare il calcolo, al prezzo di congelare alcuni punti di linearizzazione. Gli outlier, come le false chiusure del loop, richiedono funzioni di costo robuste o vincoli commutabili, poiché una semplice soluzione ai minimi quadrati può essere spostata molto lontano da un solo fattore errato.

Perché è importante

I grafi dei fattori sono il linguaggio comune dello SLAM moderno e della stima dello stato multisensore. Consentono agli ingegneri di aggiungere sensori, distribuzioni a priori e vincoli senza riprogettare lo stimatore, e rendono esplicite le ipotesi di rumore e di indipendenza che ogni misura comporta.

Applicazioni principali

Back-end SLAM per sistemi LiDAR, visivi e visivo-inerziali
Fusione multisensore di IMU, GPS, odometria delle ruote e visione
Calibrazione dei parametri estrinseci di telecamera, LiDAR e IMU
Structure from motion e bundle adjustment
Ottimizzazione di traiettorie formulata come inferenza probabilistica
Mappatura multi-robot con vincoli condivisi

In pratica

Un veicolo terrestre si localizza con il georadar rispetto a una mappa del sottosuolo registrata in precedenza. Ogni passo temporale aggiunge una variabile di posa; l'odometria inerziale o delle ruote aggiunge un fattore tra pose consecutive; e un allineamento del radar con la mappa precedente aggiunge un fattore sulla posa corrente. Invece di scrivere a mano un modello del sensore per il radar, il fattore di misura può essere appreso dai dati, come nei lavori sui modelli di grafo dei fattori appresi per la localizzazione con GPR. Il gruppo esegue uno smoother incrementale affinché la posa più recente sia disponibile in tempo reale e, dopo ogni percorso di prova, esamina i residui dei fattori per individuare i tipi di misura i cui modelli di rumore sono impostati troppo stretti o troppo larghi.

Idee sbagliate comuni

Un grafo dei fattori è un solutore — è una rappresentazione del problema; il solutore, come Gauss-Newton batch o lo smoothing incrementale, è una scelta separata
I grafi dei fattori descrivono solo grafi delle pose — possono contenere punti di riferimento, velocità, bias, parametri di calibrazione e modelli di misura arbitrari
Aggiungere più fattori migliora sempre la stima — fattori con modelli di rumore sbagliati o misure anomale possono corrompere l'intera soluzione se non si usano costi robusti

Chi dovrebbe saperlo

Ingegneri di SLAM e di stima dello stato
Sviluppatori di fusione sensoriale e navigazione
Ricercatori che lavorano sull'inferenza probabilistica in robotica
Team di localizzazione per veicoli autonomi

Tecnologie correlate

GTSAM
iSAM2
g2o
Ceres Solver
Bundle adjustment (compensazione a fasci)
Algoritmo di Levenberg-Marquardt (Levenberg-Marquardt Algorithm)

Termini correlati

Progetti correlati

LingBot-Map: Streaming 3D reconstruction from one camera: pose and depth for every frame, live
oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO
DimOS: Python robotics runtime with agents, SLAM and teleop, no ROS required
evo: De-facto standard tool for evaluating and plotting odometry/SLAM trajectories

Video

Factor Graph - 5 Minutes with Cyrill: Cyrill Stachniss
MIT Robotics - Frank Dellaert - Factor Graphs for Perception and Action: MIT Robotics