InV2IWO: Invariant Vanishing Point-Aided Visual–Inertial–Wheel Odometry in Structural Environments

Abstract

Visual-inertial odometry (VIO) commonly suffers from observability degeneration for ground robots, which can be addressed by introducing wheel odometers. However, wheel-aided VIO may still be susceptible to pose drift. In this article, we propose a consistent visual-inertial–wheel odometry (VIWO) leveraging the vanishing point (VP) to address the challenge. First, we introduce the right invariant visual-inertial–wheel filter, which models the estimated extended pose as a matrix Lie group and maintains the estimator consistency. VPs extracted from the structural environment are estimated online, where a unit vector representation and error model on manifold are adopted. Furthermore, we adopt a delayed initialization strategy to enhance the accuracy of VP initialization. We analyze the proposed VP-aided VIWO system and identify that the unobservable directions depend on the estimated VP only. The effectiveness of the proposed method is validated by Monte Carlo simulation, and we also evaluate and compare the performance of our proposed algorithm with state-of-the-art algorithms on public datasets. The results demonstrate that our method outperforms other algorithms by a large margin.