Description
Simultaneous localization and mapping (SLAM) is the problem of jointly estimating the state (such as pose, velocity, IMU biases etc) of a robot (localization) along with a map of the environment of the robot (mapping). The typical estimators used for landmark-based visual-inertial SLAM fall into one of two categories: batch optimization methods, and filtering methods. This paper analyzes the landmark-based SLAM problem through the lens of nonlinear optimization, and presents a framework that can be used to analyze, implement, and flexibly interpolate between a vast variety of SLAM algorithms. In particular, we demonstrate the equivalence between filtering based algorithms such as the Multi-state constraint Kalman filter (MSCKF) and optimization based algorithms like the sliding window filter. We present a re-interpretation of the MSCKF in terms of nonlinear optimization, and present a novel implementation based on it. We empirically compare the performance of sliding window filters and MSCKF on challenging image sequences, and use the proposed re-interpretation to explain the relative performance characteristics of the two classes of algorithms.