Efficient Factor Graph Fusion for Multi-Robot Mapping and Beyond

Abstract

This work presents a novel method to efficiently factorize the combination of multiple factor graphs having common variables of estimation. Variable ordering, a well-known variable elimination technique in linear algebra is employed to efficiently solve a factor graph. Our primary contribution in this work is to reuse the variable ordering of the graphs being combined to find the ordering of the fused graph called fusion ordering. By reusing the variable ordering of the parent graphs we were able to produce an order-of-magnitude difference in the time required for solving the fused graph. A formal verification is provided to show that the proposed strategy does not violate any of the relevant standards. The fusion ordering is experimented on the standard dataset used in the sparse linear algebra community called SuiteSparse [1]. Recent factor graph formulation for Simultaneous Localization and Mapping (SLAM) like Incremental Smoothing and Mapping (ISAM) using the Bayes tree has been very successful and garnered much attention. In the case of mapping, multi-robot system has a great advantage over a single robot that provides faster map coverage and better estimation quality. We also demonstrate the improvement of our ordering scheme on a real-world multi-robot AP Hill dataset [2].