Asynchronous and Parallel Distributed Pose Graph Optimization

Abstract

We present Asynchronous Stochastic Parallel Pose Graph Optimization (ASAPP), the first asynchronous algorithm for distributed pose graph optimization (PGO) in multirobot simultaneous localization and mapping. By enabling robots to optimize their local trajectory estimates without synchronization, ASAPP offers resiliency against communication delays and alleviates the need to wait for stragglers in the network. Furthermore, the same algorithm can be used to solve the so-called rank-restricted semidefinite relaxations of PGO, a crucial class of non-convex Riemannian optimization problems at the center of recent PGO solvers with global optimality guarantees. Under bounded delay, we establish the global firstorder convergence of ASAPP using a sufficiently small stepsize. The derived stepsize depends on the worst-case delay and inherent problem sparsity, and furthermore matches known result for synchronous algorithms when delay is zero. Numerical evaluations on both simulated and real-world SLAM datasets demonstrate the speedup achieved with ASAPP and show the algorithm's resilience against a wide range of communication delays in practice. I. INTRODUCTIONMulti-robot simultaneous localization and mapping (SLAM) is a fundamental capability for many real-world robotic applications. Pose graph optimization (PGO) is the backbone of state of the art approaches to multirobot SLAM, which fuses individual trajectories together and endows participating robots with a common spatial understanding of the environment. Many approaches to multi-robot PGO require the centralized processing of observations at a base station, which is communication intensive and vulnerable to single point of failure. In contrast, decentralized approaches are favorable as they effectively mitigate communication, privacy, and vulnerability concerns associated with centralization.Recent works on distributed PGO have achieved important progress; see e.g., [1], [2] and the references therein. However, to the best of our knowledge, existing distributed algorithms are inherently synchronous, which necessitates that robots, for instance, pass messages over the network or wait at predetermined points, in order to ensure upto-date information sharing during distributed optimization. Doing so may incur considerable communication overhead and increase the complexity of implementation. On the other hand, simply dropping synchronization in the execution of Supported in part by ARL DCIST under Cooperative Agreement Number W911NF-17-2-0181 and by NASA Convergent Aeronautics Solutions project Design Environment for Novel Vertical Lift Vehicles (DELIVER).

GeoD: Consensus-based Geodesic Distributed Pose Graph Optimization

“…A recent distributed pose graph solver [22] provides a certificate of optimality using a sparse semidefinite relaxation, similarly to SE-Sync. Finally, another recent solver is asynchronous and distributed [23], but requires the pose graph updates to be executed separately, considering a single robot at a given time. Differently, our algorithm is able to update all the graph poses simultaneously in the fashion of consensus-based estimation methods.…”