Consistency analysis for sliding-window visual odometry

In this paper we focus on the problem of visual odometry, i.e., the task of tracking the pose of a moving platform using visual measurements. In recent years, several VO algorithms have been proposed that employ nonlinear minimization in a sliding window of poses for this task. Through the use of iterative re-linearization, these methods are capable of successfully addressing the nonlinearity of the measurement models, and have become the de-facto standard for high-precision VO. In this work, we conduct an analysis of the properties of marginalization, which is the process through which older states are removed from the sliding window. This analysis shows that the standard way of marginalizing older poses results in an erroneous change in the rank of the measurements´ information matrix, and leads to underestimation of the uncertainty of the state estimates. Based on the analytical results, we also propose a simple modification of the way in which the measurement Jacobians are computed. This modification avoids the above problem, and results in an algorithm with superior accuracy, as demonstrated in both simulation tests and real-world experiments.