Factor Graph

• Catégorie: State estimation & navigation › Localisation
• Mots-clés: state-estimation, slam, probabilistic-inference, optimization, sensor-fusion

Un graphe biparti qui représente une distribution de probabilité comme un produit de facteurs, reliant des variables inconnues comme les poses et les amers aux mesures et aux a priori qui les contraignent.

Définition

Un graphe de facteurs (factor graph) comporte deux types de nœuds. Les nœuds variables représentent les inconnues à estimer, comme les poses du robot, les vitesses, les biais des capteurs ou les positions des amers. Les nœuds facteurs représentent des éléments d'information à leur sujet, comme un a priori, une mesure d'odométrie entre deux poses ou une observation d'un amer par une caméra, et chaque facteur n'est relié qu'aux variables qu'il concerne. Le graphe décrit une distribution jointe égale au produit de tous ses facteurs, ce qui rend explicite la structure d'un problème : quelles inconnues sont liées, et quelles mesures sont des contraintes creuses et locales.

Les graphes de facteurs ont été présentés comme cadre général d'inférence par Kschischang, Frey et Loeliger (2001) et largement adoptés en robotique, en grande partie grâce aux travaux de Dellaert et Kaess sur le lissage et la cartographie. Avec des modèles de bruit gaussiens, trouver les valeurs les plus probables de toutes les variables étant donné tous les facteurs est un problème de moindres carrés non linéaires : chaque facteur apporte un résidu pondéré par sa matrice d'information. Les solveurs linéarisent les résidus, exploitent le caractère creux du graphe pour factoriser efficacement le système linéaire obtenu, et itèrent avec Gauss-Newton ou Levenberg-Marquardt. Les solveurs incrémentaux comme iSAM2 mettent à jour la solution à l'arrivée de nouveaux facteurs sans tout résoudre à nouveau. GTSAM implémente directement les graphes de facteurs, et des bibliothèques générales de moindres carrés comme g2o et Ceres sont utilisées pour les mêmes types de problèmes.

Le principal attrait pratique est la modularité. Un nouveau capteur devient un nouveau type de facteur avec son propre résidu et son propre modèle de bruit, si bien que la préintégration IMU, le GPS, l'odométrie des roues, les recalages de scans LiDAR, les fermetures de boucle et les modèles de mesure appris peuvent tous coexister dans un seul estimateur. L'optimisation de graphe de poses (pose graph optimization) est un cas particulier dans lequel chaque variable est une pose et chaque facteur une contrainte de pose relative ; un graphe de facteurs général peut aussi contenir des amers, des vitesses, des biais et des paramètres d'étalonnage. Les lisseurs à retard fixe marginalisent les anciennes variables pour borner le calcul, au prix du gel de certains points de linéarisation. Les valeurs aberrantes, comme les fausses fermetures de boucle, nécessitent des fonctions de coût robustes ou des contraintes commutables, car une solution par moindres carrés simple peut être fortement déviée par un seul mauvais facteur.

Pourquoi c'est important

Les graphes de facteurs sont le langage commun du SLAM moderne et de l'estimation d'état multicapteur. Ils permettent aux ingénieurs d'ajouter des capteurs, des a priori et des contraintes sans reconcevoir l'estimateur, et ils rendent explicites les hypothèses de bruit et d'indépendance que porte chaque mesure.

Applications principales

• Modules arrière de SLAM pour systèmes LiDAR, visuels et visuels-inertiels
• Fusion multicapteur d'IMU, de GPS, d'odométrie des roues et de vision
• Étalonnage des paramètres extrinsèques de caméras, de LiDAR et d'IMU
• Structure from motion et ajustement de faisceaux
• Optimisation de trajectoire formulée comme une inférence probabiliste
• Cartographie multirobot avec contraintes partagées

En pratique

Un véhicule terrestre se localise par géoradar (GPR) par rapport à une carte du sous-sol enregistrée au préalable. Chaque pas de temps ajoute une variable de pose ; l'odométrie inertielle ou des roues ajoute un facteur entre poses consécutives ; et un recalage radar par rapport à la carte préalable ajoute un facteur sur la pose courante. Au lieu d'écrire à la main un modèle de capteur pour le radar, le facteur de mesure peut être appris à partir de données, comme dans les travaux sur des modèles de graphes de facteurs appris pour la localisation par GPR. L'équipe exécute un lisseur incrémental pour que la dernière pose soit disponible en temps réel et, après chaque essai sur le terrain, examine les résidus des facteurs pour repérer les types de mesures dont les modèles de bruit sont réglés trop serrés ou trop lâches.

Idées reçues

• Un graphe de facteurs est un solveur — c'est une représentation du problème ; le solveur, comme Gauss-Newton par lots ou le lissage incrémental, est un choix distinct
• Les graphes de facteurs ne décrivent que des graphes de poses — ils peuvent contenir des amers, des vitesses, des biais, des paramètres d'étalonnage et des modèles de mesure arbitraires
• Ajouter des facteurs améliore toujours l'estimation — des facteurs aux modèles de bruit erronés ou des mesures aberrantes peuvent corrompre toute la solution si l'on n'utilise pas de coûts robustes

Qui devrait le savoir

• Ingénieurs en SLAM et en estimation d'état
• Développeurs en fusion de capteurs et en navigation
• Chercheurs travaillant sur l'inférence probabiliste en robotique
• Équipes de localisation de véhicules autonomes

Technologies associées

• GTSAM
• iSAM2
• g2o
• Ceres Solver
• Ajustement de faisceaux (Bundle Adjustment)
• Algorithme de Levenberg-Marquardt (Levenberg-Marquardt Algorithm)

Termes associés

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

Projets associés

• 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

Vidéos

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