Object-based Visual-Inertial Navigation System on Matrix Lie Group

In this paper, we propose a novel object-based visual-inertial navigation system fully embedded in a matrix Lie group and built upon the invariant Kalman filtering theory. Specifically, we focus on relative pose measurements of objects and derive an error equation at the associated tangent space. We prove that the observability property does not suffer from the filter inconsistency and nonlinear error terms are identically zero at the object initialization. A thorough Monte-Carlo simulation reveals that our approach yields consistent estimates and is very robust to a large initial state uncertainty. Further-more, we demonstrate a real-world application to the KITTI dataset with a deep neural network-based 3D object detector. Experimental results report that noises on pose measurements follow a Gaussian-like density matching our assumption. The proposed method improves the localization and object global mapping accuracy by probabilistically accounting for inertial readings and object pose uncertainties at multiple views.