Pose Graph Optimization

• Detto anche: PGO, pose-graph SLAM, pose graph optimisation
• Categoria: State estimation & navigation › Localisation
• Tag: slam, state-estimation, optimization, localization, mapping, factor-graphs

Una tecnica di back end SLAM che raffina la traiettoria di un robot regolando un grafo delle pose in modo che i vincoli di odometria e di chiusura del loop tra di esse concordino il più possibile.

Definizione

Ottimizzazione del grafo delle pose (pose graph optimization) è il processo di stima della traiettoria di un robot tramite la risoluzione di un problema ai minimi quadrati su un grafo. Ogni nodo del grafo è una posa del robot (una posizione e un orientamento in un dato istante), e ogni arco è una trasformazione relativa misurata tra due pose, insieme alla sua incertezza, di solito espressa come matrice di informazione. Gli archi provengono dall'odometria tra pose consecutive e dalle chiusure del loop, in cui il robot riconosce un luogo che ha già visitato.

Poiché ogni misura è rumorosa, gli archi non concordano mai perfettamente: concatenare gli archi di odometria lungo un loop non riporta il robot esattamente dove un arco di chiusura del loop dice che dovrebbe trovarsi. L'ottimizzazione del grafo delle pose trova l'insieme di pose che minimizza l'errore quadratico pesato totale su tutti gli archi, distribuendo la correzione sull'intera traiettoria in proporzione alla certezza di ciascuna misura.

Il problema è non lineare perché le rotazioni non si combinano in modo additivo, quindi lo si risolve in modo iterativo con metodi come Gauss-Newton o Levenberg-Marquardt, linearizzando attorno alla stima corrente a ogni passo. I sistemi lineari risultanti sono sparsi, poiché ogni posa è collegata solo a poche altre, e i solutori sparsi rendono trattabili grafi con molte migliaia di pose. Librerie open source come g2o, GTSAM e Ceres Solver sono comunemente usate per questo passo.

L'ottimizzazione del grafo delle pose è il back end della maggior parte dei sistemi SLAM basati su grafo. Si differenzia dal bundle adjustment perché i landmark non sono mantenuti come variabili: si ottimizzano solo le pose, il che è più veloce ma scarta parte dell'informazione. Il suo noto punto debole è la sensibilità alle chiusure del loop errate, motivo per cui funzioni di costo robuste e reiezione degli outlier sono aggiunte standard.

Perché è importante

L'odometria e l'odometria visiva derivano senza limiti nel tempo, e l'ottimizzazione del grafo delle pose è il modo standard per eliminare quella deriva accumulata una volta trovate le chiusure del loop. Trasforma un insieme di misure localmente accurate ma globalmente incoerenti in una traiettoria e una mappa coerenti su un intero edificio o sito.

Applicazioni principali

• Back end SLAM basati su grafo per la mappatura con LiDAR e con fotocamere
• Eliminazione della deriva accumulata dall'odometria delle ruote, visiva e visivo-inerziale
• Mappatura multi-sessione, unendo mappe registrate in momenti diversi
• Mappatura multi-robot, combinando le traiettorie di più robot in un unico sistema di riferimento

In pratica

In uno stack SLAM tipico, LiDAR o visivo, il front end aggiunge un nuovo nodo ogni volta che il robot percorre una data distanza o ruota di un dato angolo e lo collega al nodo precedente con un arco di odometria. Un modulo di riconoscimento dei luoghi propone chiusure del loop, che vengono verificate geometricamente (per esempio con ICP o con il matching delle feature) prima di essere aggiunte come archi. Il back end esegue poi un ottimizzatore come g2o o GTSAM, in batch dopo ogni chiusura del loop oppure in modo incrementale (come con iSAM2), e le pose corrette vengono usate per ricostruire la mappa. Buona parte dello sforzo di taratura va nelle covarianze assegnate a ciascun arco e nei kernel robusti come Huber o Cauchy, perché una sola falsa chiusura del loop con alta confidenza può piegare l'intera mappa.

Idee sbagliate comuni

• L'ottimizzazione del grafo delle pose costruisce la mappa — corregge solo le pose; la mappa viene ricostruita in seguito collocando i dati dei sensori nelle pose corrette
• Più chiusure del loop aiutano sempre — una sola chiusura del loop errata può distorcere l'intera traiettoria, quindi le chiusure vanno verificate e il loro peso ridotto con costi robusti
• È la stessa cosa del bundle adjustment — il bundle adjustment ottimizza anche le posizioni dei landmark, mentre un grafo delle pose mantiene solo pose e vincoli relativi

Chi dovrebbe saperlo

• Ingegneri SLAM e di localizzazione
• Sviluppatori di robot mobili e veicoli autonomi
• Ingegneri di visione artificiale che lavorano su odometria visiva e mappatura
• Studenti che studiano robotica probabilistica e stima dello stato

Tecnologie correlate

• g2o
• GTSAM
• Ceres Solver
• Grafi di fattori (Factor Graphs)
• Bundle Adjustment
• Algoritmo di Levenberg-Marquardt (Levenberg-Marquardt Algorithm)

Termini correlati

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

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
• OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
• Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots
• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO

Video

• Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3: MATLAB