Square Root, Sequential Visual Odometry for Constant-Time Navigation and Mapping

Abstract

View Video Presentation: https://doi.org/10.2514/6.2022-1224.vid Sparse information filters for solving the Simultaneous Localization and Mapping (SLAM) problem with constant-time complexity have always come with the caveat that approximations of the inversion of the information matrix are necessary to recover the state vector, both for real-time user feedback and in order to linearize the dynamics and measurement models. This work presents the an exact, constant-time sparse filter formulation for feature-based visual odometry that, by construction, avoids the costly inversion operation or approximation thereof required in all previous formulations while still retaining sparsity as well as an inertially-referenced probabilistic map of the environment. The term ``visual odometry'' is adopted here because any analysis of loop-closures is left to future work. In tandem with the presented filtering architecture, a novel modification to any information filter, necessary to utilize an inverse depth measurement model for visual features, is described and demonstrated. A simple, 2-dimensional robot simulation is used to test the new filter and compare it to an analogous EKF implementation based on computation time and accuracy.