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.

Authors

• Kevin Eckenhoff
• Liam Paull
• Guoquan Huang 0001

Keywords

• Markov processes
• Simultaneous localization and mapping
• Maximum likelihood estimation
• Jacobian matrices
• Optimization
• Topology
• Convex functions
• Node Removal
• Graph-based SLAM
• Convex Optimization
• Choice Of Point
• Sparse Factorization
• Markov Blanket
• Reference Frame
• Approximate Distribution
• Kullback-Leibler
• Estimation Problem
• Global Estimates
• Nodes In The Graph
• Local Estimates
• Point Process
• Hessian Matrix
• Fisher Information
• Subsequent Optimization
• Global Frame
• Sparsity Level
• Nonlinear Least Squares Problem
• Factor Graph
• Sparsity Parameter
• Off-diagonal Blocks
• Sparse Estimation
• Local Reference Frame
• Cost Function
• Root Mean Square Error
• Future Optimization
• Gaussian Noise