Factor Graph

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

Um grafo bipartido que representa uma distribuição de probabilidade como um produto de fatores, ligando variáveis desconhecidas, como poses e marcos, às medições e distribuições a priori que as restringem.

Definição

Um grafo de fatores (factor graph) tem dois tipos de nós. Os nós de variável representam as incógnitas a estimar, como poses do robô, velocidades, vieses de sensores ou posições de marcos. Os nós de fator representam informações sobre elas, como uma distribuição a priori, uma medição de odometria entre duas poses ou uma observação de um marco por câmera, e cada fator se conecta apenas às variáveis que envolve. O grafo descreve uma distribuição conjunta igual ao produto de todos os seus fatores, o que torna explícita a estrutura de um problema: quais incógnitas estão ligadas e quais medições são restrições esparsas e locais.

Os grafos de fatores foram apresentados como um arcabouço geral de inferência por Kschischang, Frey e Loeliger (2001) e foram amplamente adotados na robótica, em grande parte por meio do trabalho de Dellaert e Kaess sobre suavização e mapeamento. Com modelos de ruído gaussianos, encontrar os valores mais prováveis de todas as variáveis dados todos os fatores é um problema de mínimos quadrados não lineares: cada fator contribui com um resíduo ponderado por sua matriz de informação. Os solvers linearizam os resíduos, exploram a esparsidade do grafo para fatorar com eficiência o sistema linear resultante e iteram com Gauss-Newton ou Levenberg-Marquardt. Solvers incrementais como o iSAM2 atualizam a solução à medida que novos fatores chegam, sem resolver tudo de novo do zero. O GTSAM implementa grafos de fatores diretamente, e bibliotecas gerais de mínimos quadrados como g2o e Ceres são usadas para os mesmos tipos de problema.

O principal atrativo prático é a modularidade. Um sensor novo se torna um novo tipo de fator com seu próprio resíduo e modelo de ruído, de modo que a pré-integração da IMU, o GPS, a odometria de rodas, os alinhamentos de varreduras LiDAR, os fechamentos de loop e os modelos de medição aprendidos podem conviver em um único estimador. A otimização de grafo de poses é um caso especial em que toda variável é uma pose e todo fator é uma restrição de pose relativa; um grafo de fatores geral também pode conter marcos, velocidades, vieses e parâmetros de calibração. Os suavizadores de atraso fixo marginalizam as variáveis antigas para limitar o custo computacional, ao preço de congelar alguns pontos de linearização. Outliers, como falsos fechamentos de loop, exigem funções de custo robustas ou restrições comutáveis, já que uma solução de mínimos quadrados simples pode ser puxada para longe por um único fator ruim.

Por que importa

Os grafos de fatores são a linguagem comum do SLAM moderno e da estimação de estado com múltiplos sensores. Eles permitem que os engenheiros acrescentem sensores, distribuições a priori e restrições sem reprojetar o estimador, e tornam explícitas as suposições de ruído e de independência que cada medição carrega.

Principais aplicações

Back-ends de SLAM para sistemas LiDAR, visuais e visual-inerciais
Fusão multissensor de IMU, GPS, odometria de rodas e visão
Calibração dos parâmetros extrínsecos de câmera, LiDAR e IMU
Structure from motion e ajuste de feixes (bundle adjustment)
Otimização de trajetória formulada como inferência probabilística
Mapeamento multirrobô com restrições compartilhadas

Na prática

Um veículo terrestre se localiza com radar de penetração no solo em relação a um mapa de subsuperfície gravado anteriormente. Cada passo de tempo acrescenta uma variável de pose; a odometria inercial ou de rodas acrescenta um fator entre poses consecutivas; e um alinhamento do radar com o mapa prévio acrescenta um fator sobre a pose atual. Em vez de escrever à mão um modelo de sensor para o radar, o fator de medição pode ser aprendido a partir de dados, como em trabalhos sobre modelos de grafos de fatores aprendidos para localização com GPR. A equipe executa um suavizador incremental para que a pose mais recente esteja disponível em tempo real e, após cada percurso de teste, inspeciona os resíduos dos fatores para encontrar tipos de medição cujos modelos de ruído estão ajustados apertados ou folgados demais.

Equívocos comuns

Um grafo de fatores é um solver — ele é uma representação do problema; o solver, como Gauss-Newton em lote ou suavização incremental, é uma escolha separada
Grafos de fatores só descrevem grafos de poses — eles podem conter marcos, velocidades, vieses, parâmetros de calibração e modelos de medição arbitrários
Acrescentar mais fatores sempre melhora a estimativa — fatores com modelos de ruído errados ou medições discrepantes podem corromper toda a solução, a menos que sejam usados custos robustos

Quem deve saber

Engenheiros de SLAM e de estimação de estado
Desenvolvedores de fusão de sensores e navegação
Pesquisadores que trabalham com inferência probabilística em robótica
Equipes de localização de veículos autônomos

Tecnologias relacionadas

GTSAM
iSAM2
g2o
Ceres Solver
Ajuste de feixes (Bundle Adjustment)
Algoritmo de Levenberg-Marquardt (Levenberg-Marquardt Algorithm)

Termos relacionados

Projetos relacionados

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

Vídeos

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