Certifiably correct SLAM

Abstract

The ability to construct an accurate model of the environment is an essential capability for mobile autonomous systems, enabling such fundamental functions as planning, navigation, and manipulation. However, the general form of this problem, simultaneous localization and mapping (SLAM), is typically formulated as a maximum-likelihood estimation (MLE) that requires solving a nonconvex nonlinear program, which is computationally hard. Current state-of-the-art SLAM algorithms address this difficulty by applying fast local optimization methods to compute a critical point of the MLE. While this approach has enabled significant advances in SLAM as a practical technology by admitting the development of fast and scalable estimation methods, it provides no guarantees on the quality of the recovered estimates. This lack of reliability in existing SLAM algorithms in turn presents a serious barrier to the development of robust autonomous systems generally. To address this problem, in this thesis we develop a suite of algorithms for SLAM that preserve the computational efficiency of existing state-of-the-art methods while additionally providing explicit performance guarantees. Our contribution is threefold. First, we develop a provably reliable method for performing fast local optimization in the online setting. Our algorithm, Robust Incremental least-Squares Estimation (RISE), maintains the superlinear convergence rate of existing state-of-the-art online SLAM solvers while providing superior robustness to nonlinearity and numerical ill-conditioning; in particular, we prove that RISE is globally convergent under very mild hypotheses (namely, that the objective is twice-continuously differentiable with bounded sublevel sets). We show experimentally that RISE's enhanced convergence properties lead to dramatically improved performance versus alternative methods on SLAM problems exhibiting strong nonlinearities, such as those encountered in visual mapping or when employing robust cost functions. Next, we address the lack of a priori performance guarantees when applying local optimization methods to the nonconvex SLAM MLE by proposing a post hoc verification method for computationally certifying the correctness of a recovered estimate for pose-graph SLAM. The crux of our approach is the development of a (convex) semidefinite relaxation of the SLAM MLE that is frequently exact in the low to moderate measurement noise regime. We show that when exactness holds, it is straightforward to construct an optimal solution Z* for this relaxation from an optimal solution X* of the SLAM problem; the dual solution Z* (whose optimality can be verified directly post hoc) then serves as a certificate of optimality for the solution X* from which it was constructed. Extensive evaluation on a variety of simulated and real-world pose-graph SLAM datasets shows that this verification method succeeds in certifying optimal solutions across a broad spectrum of operating conditions, including those typically encountered in application. Our final contribution is the development of SE-Sync, a pose-graph SLAM inference algorithm that employs a fast purpose-built optimization method to directly solve the aforementioned semidefinite relaxation, and thereby recover a certifiably globally optimal solution of the SLAM MLE whenever exactness holds. As in the case of our verification technique, extensive empirical evaluation on a variety of simulated and real-world datasets shows that SE-Sync is capable of recovering globally optimal pose-graph SLAM solutions across a broad spectrum of operating conditions (including those typically encountered in application), and does so at a computational cost that scales comparably with that of fast Newton-based local search techniques. Collectively, these algorithms provide fast and robust inference and verification methods for pose-graph SLAM in both the online and offline settings, and can be straightforwardly incorporated into existing concurrent filtering smoothing architectures. The result is a framework for real-time mapping and navigation that preserves the computational speed of current state-of-the-art techniques while delivering certifiably correct solutions.