Pose Graph Optimization

• Aussi appelé: PGO, pose-graph SLAM, pose graph optimisation
• Catégorie: State estimation & navigation › Localisation
• Mots-clés: slam, state-estimation, optimization, localization, mapping, factor-graphs

Une technique de back-end SLAM qui affine la trajectoire d'un robot en ajustant un graphe de poses afin que les contraintes d'odométrie et de fermeture de boucle entre elles concordent le mieux possible.

Définition

Optimisation de graphe de poses (pose graph optimization) : processus d'estimation de la trajectoire d'un robot par la résolution d'un problème de moindres carrés sur un graphe. Chaque nœud du graphe est une pose du robot (une position et une orientation à un instant donné), et chaque arête est une transformation relative mesurée entre deux poses, accompagnée de son incertitude, généralement exprimée par une matrice d'information. Les arêtes proviennent de l'odométrie entre poses consécutives et des fermetures de boucle, lorsque le robot reconnaît un lieu qu'il a déjà visité.

Comme toute mesure est bruitée, les arêtes ne concordent jamais parfaitement : enchaîner les arêtes d'odométrie autour d'une boucle ne ramène pas le robot exactement là où une arête de fermeture de boucle indique qu'il devrait être. L'optimisation de graphe de poses trouve l'ensemble de poses qui minimise l'erreur quadratique pondérée totale sur toutes les arêtes, en répartissant la correction sur toute la trajectoire proportionnellement à la certitude de chaque mesure.

Le problème est non linéaire car les rotations ne se composent pas de façon additive ; il est donc résolu itérativement avec des méthodes comme Gauss-Newton ou Levenberg-Marquardt, en linéarisant autour de l'estimation courante à chaque pas. Les systèmes linéaires obtenus sont creux, puisque chaque pose n'est reliée qu'à quelques autres, et les solveurs creux rendent traitables des graphes de plusieurs milliers de poses. Des bibliothèques open source comme g2o, GTSAM et Ceres Solver sont couramment utilisées pour cette étape.

L'optimisation de graphe de poses est le back-end de la plupart des systèmes SLAM fondés sur des graphes. Elle diffère de l'ajustement de faisceaux en ce que les amers ne sont pas conservés comme variables : seules les poses sont optimisées, ce qui est plus rapide mais écarte une partie de l'information. Sa faiblesse connue est la sensibilité aux fausses fermetures de boucle, c'est pourquoi les fonctions de coût robustes et le rejet des valeurs aberrantes sont des ajouts standard.

Pourquoi c'est important

L'odométrie et l'odométrie visuelle dérivent sans limite au fil du temps, et l'optimisation de graphe de poses est le moyen standard d'éliminer cette dérive accumulée une fois des fermetures de boucle trouvées. Elle transforme un ensemble de mesures localement précises mais globalement incohérentes en une trajectoire et une carte cohérentes à l'échelle de tout un bâtiment ou d'un site.

Applications principales

• Back-ends SLAM fondés sur des graphes pour la cartographie par LiDAR et par caméra
• Élimination de la dérive accumulée de l'odométrie par roues, visuelle et visuo-inertielle
• Cartographie multisession, fusionnant des cartes enregistrées à des moments différents
• Cartographie multirobot, combinant les trajectoires de plusieurs robots dans un même repère

En pratique

Dans une pile SLAM LiDAR ou visuelle typique, le front-end ajoute un nouveau nœud chaque fois que le robot a parcouru une distance ou un angle donnés et le relie au nœud précédent par une arête d'odométrie. Un module de reconnaissance de lieux propose des fermetures de boucle, qui sont vérifiées géométriquement (par exemple avec ICP ou la mise en correspondance de caractéristiques) avant d'être ajoutées comme arêtes. Le back-end exécute ensuite un optimiseur comme g2o ou GTSAM, soit par lots après chaque fermeture de boucle, soit de manière incrémentale (comme avec iSAM2), et les poses corrigées servent à reconstruire la carte. Une grande partie de l'effort de réglage porte sur les covariances attribuées à chaque arête et sur les noyaux robustes comme Huber ou Cauchy, car une seule fausse fermeture de boucle avec une confiance élevée peut courber toute la carte.

Idées reçues

• L'optimisation de graphe de poses construit la carte — elle ne corrige que les poses ; la carte est reconstruite ensuite en plaçant les données des capteurs aux poses corrigées
• Plus de fermetures de boucle aident toujours — une seule fermeture de boucle erronée peut déformer toute la trajectoire, les fermetures doivent donc être vérifiées et sous-pondérées par des coûts robustes
• C'est la même chose que l'ajustement de faisceaux — l'ajustement de faisceaux optimise aussi la position des amers, tandis qu'un graphe de poses ne conserve que les poses et les contraintes relatives

Qui devrait le savoir

• Ingénieurs SLAM et localisation
• Développeurs de robots mobiles et de véhicules autonomes
• Ingénieurs en vision par ordinateur travaillant sur l'odométrie visuelle et la cartographie
• Étudiants qui apprennent la robotique probabiliste et l'estimation d'état

Technologies associées

• g2o
• GTSAM
• Ceres Solver
• Graphes de facteurs (Factor Graphs)
• Ajustement de faisceaux (Bundle Adjustment)
• Algorithme de Levenberg-Marquardt (Levenberg-Marquardt Algorithm)

Termes associés

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

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
• 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

Vidéos

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