Factor Graph

• Categoría: State estimation & navigation › Localisation
• Etiquetas: state-estimation, slam, probabilistic-inference, optimization, sensor-fusion

Un grafo bipartito que representa una distribución de probabilidad como un producto de factores y vincula las variables desconocidas, como poses y puntos de referencia, con las medidas y las distribuciones a priori que las restringen.

Definición

Un grafo de factores (factor graph) tiene dos tipos de nodos. Los nodos de variable representan las incógnitas que se quieren estimar, como poses del robot, velocidades, sesgos de sensores o posiciones de puntos de referencia. Los nodos de factor representan fragmentos de información sobre ellas, como una distribución a priori, una medida de odometría entre dos poses o una observación de un punto de referencia por una cámara, y cada factor se conecta solo con las variables que intervienen en él. El grafo describe una distribución conjunta igual al producto de todos sus factores, lo que hace explícita la estructura de un problema: qué incógnitas están vinculadas y qué medidas son restricciones dispersas y locales.

Los grafos de factores fueron formulados como marco general de inferencia por Kschischang, Frey y Loeliger (2001) y se adoptaron ampliamente en robótica, en gran parte gracias al trabajo de Dellaert y Kaess sobre suavizado y mapeo. Con modelos de ruido gaussianos, encontrar los valores más probables de todas las variables dados todos los factores es un problema de mínimos cuadrados no lineales: cada factor aporta un residuo ponderado por su matriz de información. Los solucionadores linealizan los residuos, aprovechan la dispersión del grafo para factorizar eficientemente el sistema lineal resultante e iteran con Gauss-Newton o Levenberg-Marquardt. Los solucionadores incrementales como iSAM2 actualizan la solución a medida que llegan factores nuevos sin volver a resolver desde cero. GTSAM implementa directamente los grafos de factores, y bibliotecas generales de mínimos cuadrados como g2o y Ceres se usan para el mismo tipo de problemas.

El principal atractivo práctico es la modularidad. Un sensor nuevo se convierte en un nuevo tipo de factor con su propio residuo y su propio modelo de ruido, de modo que la preintegración de IMU, el GPS, la odometría de ruedas, los emparejamientos de escaneos LiDAR, los cierres de bucle y los modelos de medida aprendidos pueden convivir en un único estimador. La optimización de grafos de poses es un caso particular en el que cada variable es una pose y cada factor una restricción de pose relativa; un grafo de factores general también puede contener puntos de referencia, velocidades, sesgos y parámetros de calibración. Los suavizadores de retardo fijo marginalizan las variables antiguas para acotar el cómputo, a costa de congelar algunos puntos de linealización. Los valores atípicos, como los falsos cierres de bucle, requieren funciones de coste robustas o restricciones conmutables, ya que una solución de mínimos cuadrados simple puede desviarse mucho por un solo factor erróneo.

Por qué importa

Los grafos de factores son el lenguaje común del SLAM moderno y de la estimación de estado multisensor. Permiten a los ingenieros añadir sensores, distribuciones a priori y restricciones sin rediseñar el estimador, y hacen explícitos los supuestos de ruido y de independencia que lleva cada medida.

Aplicaciones clave

• Back ends de SLAM para sistemas LiDAR, visuales y visual-inerciales
• Fusión multisensor de IMU, GPS, odometría de ruedas y visión
• Calibración de los parámetros extrínsecos de cámara, LiDAR e IMU
• Structure from motion y ajuste de haces (bundle adjustment)
• Optimización de trayectorias planteada como inferencia probabilística
• Mapeo multirrobot con restricciones compartidas

En la práctica

Un vehículo terrestre se localiza con georradar frente a un mapa del subsuelo registrado previamente. Cada paso de tiempo añade una variable de pose; la odometría inercial o de ruedas añade un factor entre poses consecutivas; y una correspondencia del radar con el mapa previo añade un factor sobre la pose actual. En lugar de escribir a mano un modelo de sensor para el radar, el factor de medida puede aprenderse a partir de datos, como en los trabajos sobre modelos de grafos de factores aprendidos para la localización con GPR. El equipo ejecuta un suavizador incremental para que la última pose esté disponible en tiempo real y, tras cada recorrido de prueba, inspecciona los residuos de los factores para encontrar los tipos de medida cuyos modelos de ruido están ajustados demasiado estrechos o demasiado holgados.

Ideas erróneas comunes

• Un grafo de factores es un solucionador — es una representación del problema; el solucionador, como Gauss-Newton por lotes o el suavizado incremental, es una elección aparte
• Los grafos de factores solo describen grafos de poses — pueden contener puntos de referencia, velocidades, sesgos, parámetros de calibración y modelos de medida arbitrarios
• Añadir más factores siempre mejora la estimación — los factores con modelos de ruido erróneos o las medidas atípicas pueden corromper toda la solución si no se usan costes robustos

Quién debería saberlo

• Ingenieros de SLAM y de estimación de estado
• Desarrolladores de fusión de sensores y de navegación
• Investigadores que trabajan en inferencia probabilística en robótica
• Equipos de localización de vehículos autónomos

Tecnologías relacionadas

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

Términos relacionados

• Pose Graph Optimization
• SLAM
• IMU Preintegration
• Loop Closure
• Sensor Fusion
• Tightly Coupled Sensor Fusion

Proyectos 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