Efficient Incremental SLAM via Information-Guided Gating and Selective Partial Optimization
We present an efficient incremental SLAM back-end that achieves the accuracy of full batch optimization while substantially reducing computational cost. The proposed approach combines two complementary ideas: information-guided gating (IGG) and selective partial optimization (SPO). IGG employs an information-theoretic criterion based on the log-determinant of the information matrix to quantify the contribution of new measurements, triggering global optimization only when a significant information gain is observed. This avoids unnecessary relinearization and factorization when incoming data provide little additional information. SPO executes multi-iteration Gauss-Newton (GN) updates but restricts each iteration to the subset of variables most affected by the new measurements, dynamically refining this active set until convergence. Together, these mechanisms retain all measurements to preserve global consistency while focusing computation on parts of the graph where it yields the greatest benefit. We provide a theoretical local perturbation analysis showing that, under standard regularity assumptions for GN, the proposed approach tracks full GN up to a neighborhood whose size is controlled by the approximation thresholds. Moreover, when the effective approximation error introduced by localization and screening vanishes asymptotically, it recovers the same local minimizer and asymptotic convergence rate as full GN. Extensive experiments on benchmark SLAM datasets show that our approach consistently matches the estimation accuracy of batch solvers, while achieving significant computational savings compared to conventional incremental approaches. Such efficiency is particularly important for mobile robots operating under onboard compute constraints, where timely state estimation is critical for localization, mapping, and downstream navigation and control. The results indicate that the proposed approach offers a principled balance between accuracy and efficiency, making it a robust and scalable solution for real-time robotic localization and mapping in dynamic, data-rich environments.
