Pose Graph Optimization

• Também chamado: PGO, pose-graph SLAM, pose graph optimisation
• Categoria: State estimation & navigation › Localisation
• Tags: slam, state-estimation, optimization, localization, mapping, factor-graphs

Uma técnica de back end de SLAM que refina a trajetória de um robô ajustando um grafo de poses para que as restrições de odometria e de fechamento de loop entre elas concordem o máximo possível.

Definição

Otimização de grafo de poses (pose graph optimization) é o processo de estimar a trajetória de um robô resolvendo um problema de mínimos quadrados sobre um grafo. Cada nó do grafo é uma pose do robô (uma posição e uma orientação em um instante), e cada aresta é uma transformação relativa medida entre duas poses, junto com sua incerteza, geralmente expressa como uma matriz de informação. As arestas vêm da odometria entre poses consecutivas e dos fechamentos de loop, em que o robô reconhece um lugar que já visitou.

Como toda medição tem ruído, as arestas nunca concordam perfeitamente: encadear as arestas de odometria ao redor de um loop não traz o robô de volta exatamente para onde uma aresta de fechamento de loop diz que ele deveria estar. A otimização de grafo de poses encontra o conjunto de poses que minimiza o erro quadrático ponderado total em todas as arestas, distribuindo a correção por toda a trajetória na proporção da certeza de cada medição.

O problema é não linear porque as rotações não se combinam de forma aditiva, então ele é resolvido iterativamente com métodos como Gauss-Newton ou Levenberg-Marquardt, linearizando em torno da estimativa atual a cada passo. Os sistemas lineares resultantes são esparsos, já que cada pose se conecta a apenas algumas outras, e solucionadores esparsos tornam tratáveis grafos com muitos milhares de poses. Bibliotecas de código aberto como g2o, GTSAM e Ceres Solver são comumente usadas nessa etapa.

A otimização de grafo de poses é o back end da maioria dos sistemas de SLAM baseados em grafos. Ela difere do bundle adjustment porque os pontos de referência não são mantidos como variáveis: apenas as poses são otimizadas, o que é mais rápido mas descarta parte da informação. Sua fraqueza conhecida é a sensibilidade a fechamentos de loop errados, por isso funções de custo robustas e rejeição de outliers são acréscimos padrão.

Por que importa

A odometria e a odometria visual derivam sem limite com o tempo, e a otimização de grafo de poses é a forma padrão de remover essa deriva acumulada assim que fechamentos de loop são encontrados. Ela transforma um conjunto de medições localmente precisas, mas globalmente inconsistentes, em uma trajetória e um mapa consistentes em todo um edifício ou local.

Principais aplicações

• Back ends de SLAM baseado em grafos para mapeamento com LiDAR e com câmeras
• Remoção da deriva acumulada da odometria de rodas, visual e visual-inercial
• Mapeamento em várias sessões, mesclando mapas registrados em momentos diferentes
• Mapeamento multirrobô, combinando trajetórias de vários robôs em um único referencial

Na prática

Em uma pilha típica de SLAM por LiDAR ou visual, o front end adiciona um novo nó toda vez que o robô percorre uma distância ou um ângulo definidos e o conecta ao nó anterior com uma aresta de odometria. Um módulo de reconhecimento de lugares propõe fechamentos de loop, que são verificados geometricamente (por exemplo com ICP ou casamento de características) antes de serem adicionados como arestas. O back end então roda um otimizador como g2o ou GTSAM, em lote após cada fechamento de loop ou de forma incremental (como com o iSAM2), e as poses corrigidas são usadas para reconstruir o mapa. Boa parte do esforço de ajuste vai para as covariâncias atribuídas a cada aresta e para kernels robustos como Huber ou Cauchy, porque um único fechamento de loop falso com alta confiança pode entortar o mapa inteiro.

Equívocos comuns

• A otimização de grafo de poses constrói o mapa — ela corrige apenas as poses; o mapa é reconstruído depois, posicionando os dados dos sensores nas poses corrigidas
• Mais fechamentos de loop sempre ajudam — um único fechamento de loop incorreto pode distorcer toda a trajetória, por isso os fechamentos precisam ser verificados e ter o peso reduzido com custos robustos
• É o mesmo que bundle adjustment — o bundle adjustment também otimiza as posições dos pontos de referência, enquanto um grafo de poses mantém apenas poses e restrições relativas

Quem deve saber

• Engenheiros de SLAM e localização
• Desenvolvedores de robôs móveis e veículos autônomos
• Engenheiros de visão computacional que trabalham com odometria visual e mapeamento
• Estudantes que aprendem robótica probabilística e estimação de estado

Tecnologias relacionadas

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

Termos relacionados

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

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