GeoD

GeoD: Consensus-based Geodesic Distributed Pose Graph Optimization

Its fast, its guaranteed and its distributed! 

We present GeoD, a consensus-based distributed SE(3) pose graph optimization algorithm with provable convergence guarantees. This algorithm enables a group of robots, given noisy relative pose measurements, to reach agreement on the group’s SE(3) pose history in a distributed manner. 

The algorithm 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. We identify two new consistency conditions sufficient for convergence: pairwise consistency of relative rotation measurements, and minimal consistency of relative translation measurements. GeoD incorporates a simple one step distributed initialization to satisfy both conditions. 

On average, GeoD converges 20 times more quickly than a competing distributed algorithm to a value with 3.4 times less error when compared to the global minimum provided by SE-Sync. GeoD also scales more favorably with graph size, converging over 100 times faster on graphs larger than 1000 poses.

We have tested GeoD on a multi-UAV vision-based SLAM scenario, where the UAVs estimate their pose trajectories in a distributed manner using the relative poses extracted from their on board camera images. Qualitative results indicate that GeoD is better than centralized and distributed competing methods.