Decoupled, consistent node removal and edge sparsification for graph-based SLAM

Graph-based SLAM approaches have had success recently despite suffering from ever-increasing computational costs due to the need of optimizing over the entire robot trajectory. To address this issue, in this paper, we advocate the decoupling of marginalization (node removal) and sparsification (edge reduction) to allow for short-term retention of dense factors induced by marginalization while enabling us to spread the computation of these two operations over time. In particular, we analytically show that during marginalization, the correct choice of linearization points in constructing dense marginal factors is to use the relative (local), instead of global, state estimates in the Markov blanket of the marginalized node, which has lacked a general consensus in the literature. Furthermore, during sparsification, we determine an online sparse topology through sparsity-regularized convex optimization, which guides us to construct consistent sparse factors to best approximate the original dense factors across the Markov blanket. The proposed approach is tested extensively on both 2D and 3D public datasets and shown to perform competitively to the state-of-the-art algorithms.