Majorization Minimization Methods for Distributed Pose Graph Optimization

Taosha Fan, Todd D. Murphey

Abstract: We consider the problem of distributed pose graph optimization (PGO) that has important applications in multirobot simultaneous localization and mapping (SLAM). We propose the majorization minimization (MM) method for distributed PGO ($\mathsf {MM\text{--}PGO}$) that applies to a broad class of robust loss kernels. The $\mathsf {MM\text{--}PGO}$ method is guaranteed to converge to first-order critical points under mild conditions. Furthermore, noting that the $\mathsf {MM\text{--}PGO}$ method is reminiscent of proximal methods, we leverage Nesterov's method and adopt adaptive restarts to accelerate convergence. The resulting accelerated MM methods for distributed PGO—both with a master node in the network ($\mathsf {AMM\text{--}PGO}^*$) and without ($\mathsf {AMM\text{--}PGO}^{\#}$)—have faster convergence in contrast to the $\mathsf {MM\text{--}PGO}$ method without sacrificing theoretical guarantees. In particular, the $\mathsf {AMM\text{--}PGO}^{\#}$ method, which needs no master node and is fully decentralized, features a novel adaptive restart scheme and has a rate of convergence comparable to that of the $\mathsf {AMM\text{--}PGO}^*$ method using a master node to aggregate information from all the nodes. The efficacy of this work is validated through extensive applications to 2-D and 3-D SLAM benchmark datasets and comprehensive comparisons against existing state-of-the-art methods, indicating that our MM methods converge faster and result in better solutions to distributed PGO.

External IDs:dblp:journals/trob/FanM24