GeoD: Consensus-based Geodesic Distributed Pose Graph Optimization

Understand

We present a consensus-based distributed pose graph optimization algorithm for obtaining an estimate of the 3D translation and rotation of each pose in a pose graph, given noisy relative measurements between poses.

• The algorithm, called GeoD, implements a continuous time distributed consensus protocol to minimize the geodesic pose graph error.
• GeoD is distributed over the pose graph itself, with a separate computation thread for each node in the graph, and messages are passed only between neighboring nodes in the graph.
• We leverage tools from Lyapunov theory and multi-agent consensus to prove the convergence of the algorithm.