Pose Graph Optimization

• También llamado: PGO, pose-graph SLAM, pose graph optimisation
• Categoría: State estimation & navigation › Localisation
• Etiquetas: slam, state-estimation, optimization, localization, mapping, factor-graphs

Una técnica de back end de SLAM que refina la trayectoria de un robot ajustando un grafo de poses para que las restricciones de odometría y de cierre de bucle entre ellas concuerden lo más posible.

Definición

Optimización de grafos de poses (pose graph optimization) es el proceso de estimar la trayectoria de un robot resolviendo un problema de mínimos cuadrados sobre un grafo. Cada nodo del grafo es una pose del robot (una posición y una orientación en un instante), y cada arista es una transformación relativa medida entre dos poses, junto con su incertidumbre, expresada normalmente como una matriz de información. Las aristas proceden de la odometría entre poses consecutivas y de los cierres de bucle, en los que el robot reconoce un lugar que ya ha visitado.

Como todas las medidas tienen ruido, las aristas nunca concuerdan perfectamente: encadenar las aristas de odometría alrededor de un bucle no devuelve el robot exactamente al lugar donde una arista de cierre de bucle dice que debería estar. La optimización de grafos de poses encuentra el conjunto de poses que minimiza el error cuadrático ponderado total en todas las aristas, repartiendo la corrección por toda la trayectoria en proporción a la certeza de cada medida.

El problema es no lineal porque las rotaciones no se combinan de forma aditiva, así que se resuelve de forma iterativa con métodos como Gauss-Newton o Levenberg-Marquardt, linealizando en torno a la estimación actual en cada paso. Los sistemas lineales resultantes son dispersos, ya que cada pose se conecta solo con unas pocas, y los solucionadores dispersos hacen tratables grafos con muchos miles de poses. Bibliotecas de código abierto como g2o, GTSAM y Ceres Solver se usan habitualmente en este paso.

La optimización de grafos de poses es el back end de la mayoría de los sistemas SLAM basados en grafos. Se diferencia del ajuste de haces (bundle adjustment) en que los puntos de referencia no se mantienen como variables: solo se optimizan las poses, lo que es más rápido pero descarta parte de la información. Su debilidad conocida es la sensibilidad a los cierres de bucle erróneos, por lo que las funciones de coste robustas y el rechazo de valores atípicos son añadidos estándar.

Por qué importa

La odometría y la odometría visual derivan sin límite con el tiempo, y la optimización de grafos de poses es la forma estándar de eliminar esa deriva acumulada una vez encontrados los cierres de bucle. Convierte un conjunto de medidas localmente precisas pero globalmente incoherentes en una trayectoria y un mapa coherentes en todo un edificio o recinto.

Aplicaciones clave

• Back ends de SLAM basado en grafos para mapeo con LiDAR y con cámaras
• Eliminación de la deriva acumulada de la odometría de ruedas, visual y visual-inercial
• Mapeo en varias sesiones, fusionando mapas registrados en momentos distintos
• Mapeo multirrobot, combinando las trayectorias de varios robots en un único sistema de referencia

En la práctica

En una pila típica de SLAM con LiDAR o visual, el front end añade un nuevo nodo cada vez que el robot recorre una distancia o un ángulo determinados y lo conecta al nodo anterior con una arista de odometría. Un módulo de reconocimiento de lugares propone cierres de bucle, que se verifican geométricamente (por ejemplo con ICP o emparejamiento de características) antes de añadirse como aristas. Después, el back end ejecuta un optimizador como g2o o GTSAM, ya sea por lotes tras cada cierre de bucle o de forma incremental (como con iSAM2), y las poses corregidas se usan para reconstruir el mapa. Buena parte del esfuerzo de ajuste se dedica a las covarianzas asignadas a cada arista y a núcleos robustos como Huber o Cauchy, porque un solo cierre de bucle falso con alta confianza puede doblar todo el mapa.

Ideas erróneas comunes

• La optimización de grafos de poses construye el mapa — solo corrige las poses; el mapa se reconstruye después colocando los datos de los sensores en las poses corregidas
• Más cierres de bucle siempre ayudan — un solo cierre de bucle incorrecto puede distorsionar toda la trayectoria, así que los cierres deben verificarse y reducir su peso con costes robustos
• Es lo mismo que el ajuste de haces — el ajuste de haces también optimiza las posiciones de los puntos de referencia, mientras que un grafo de poses conserva solo las poses y las restricciones relativas

Quién debería saberlo

• Ingenieros de SLAM y de localización
• Desarrolladores de robots móviles y vehículos autónomos
• Ingenieros de visión por computador que trabajan en odometría visual y mapeo
• Estudiantes que aprenden robótica probabilística y estimación de estado

Tecnologías relacionadas

• g2o
• GTSAM
• Ceres Solver
• Grafos de factores (Factor Graphs)
• Ajuste de haces (Bundle Adjustment)
• Algoritmo de Levenberg-Marquardt (Levenberg-Marquardt Algorithm)

Términos relacionados

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

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

Vídeos

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