Cartan-Sync: Fast and Global SE(<italic>d</italic>)-Synchronization

摘要

This work addresses the fundamental problem of pose graph optimization (PGO), which is pervasive in the context of SLAM, and widely known as SE(d)-synchronization in the mathematical community. Our contribution is twofold. First, we provide a novel, elegant, and compact matrix formulation of the maximum likelihood estimation (MLE) for this problem, drawing interesting connections with the connection Laplacian of a graph object. Second, even though the MLE problem is nonconvex and computationally intractable in general, we exploit recent advances in convex relaxations of PGO and Riemannian techniques for low-rank optimization to yield an a posteriori certifiably globally optimal algorithm [A. Bandeira, “A note on probably certifiably correct algorithms,” Comptes Rendus Mathematique , vol. 354, pp. 329-333, 2016.] that is also fast and scalable. This work builds upon a fairly demanding mathematical machinery, but beyond the theoretical basis presented, we demonstrate its performance through extensive experimentation in common large-scale SLAM datasets. The proposed framework, Cartan-Sync, is up to one order of magnitude faster that the state-of-the-art SE-Sync [D. M. Rosen et al. “A certifiably correct algorithm for synchronization over the special Euclidean group,” in Proc. Int. Workshop Algorithmic Found. Robot., 2016.] in some important scenarios (e.g., the KITTI dataset). We make the code for Cartan-Sync available at bitbucket.org/jesusbriales/cartan-sync, along with some examples and guides for a friendly use by researchers in the field, hoping to promote further adoption and exploitation of these techniques in the robotics community.