Invariant Cubature Kalman Filter for Monocular Visual Inertial Odometry with Line Features

TL;DR

This paper introduces an invariant cubature Kalman filter-based visual inertial odometry method utilizing line features, enhancing robustness and accuracy in challenging environments by modeling system uncertainty on a Lie group.

Contribution

The paper develops a novel invariant CKF-based VIO algorithm incorporating line features and Lie algebra, extending traditional CKF to manifold spaces for improved robustness.

Findings

Improved accuracy over state-of-the-art methods on Euroc datasets

Enhanced robustness in low-texture and changing illumination scenes

Reduced linearization errors and better uncertainty modeling

Abstract

To achieve robust and accurate state estimation for robot navigation, we propose a novel Visual Inertial Odometry(VIO) algorithm with line features upon the theory of invariant Kalman filtering and Cubature Kalman Filter (CKF). In contrast with conventional CKF, the state of the filter is constructed by a high dimensional Matrix Lie group and the uncertainty is represented using Lie algebra. To improve the robustness of system in challenging scenes, e.g. low-texture or illumination changing environments, line features are brought into the state variable. In the proposed algorithm, exponential mapping of Lie algebra is used to construct the cubature points and the re-projection errors of lines are built as observation function for updating the state. This method accurately describes the system uncertainty in rotation and reduces the linearization error of system, which extends traditional CKF from Euclidean space to manifold. It not only inherits the advantages of invariant filtering in consistency, but also avoids the complex Jacobian calculation of high-dimensional matrix. To demonstrate the effectiveness of the proposed algorithm, we compare it with the state-of-the-art filtering-based VIO algorithms on Euroc datasets. And the results show that the proposed algorithm is effective in improving accuracy and robustness of estimation.