Consistent sparsification for graph optimization

In a standard pose-graph formulation of simultaneous localization and mapping (SLAM), due to the continuously increasing numbers of nodes (states) and edges (measurements), the graph may grow prohibitively too large for long-term navigation. This motivates us to systematically reduce the pose graph amenable to available processing and memory resources. In particular, in this paper we introduce a consistent graph sparsification scheme: i) sparsifying nodes via marginalization of old nodes, while retaining all the information (consistent relative constraints) — which is conveyed in the discarded measurements — about the remaining nodes after marginalization; and ii) sparsifying edges by formulating and solving a consistent ℓ1-regularized minimization problem, which automatically promotes the sparsity of the graph. The proposed approach is validated on both synthetic and real data.