A Real-Time CPU-GPU Embedded Implementation of a Tightly-Coupled Visual-Inertial Navigation System

In autonomous navigation technologies, the Multi-State Constraint Kalman Filter (MSCKF) is one of the most accurate and robust tightly-coupled fusion frameworks for Visual-Inertial Navigation (VIN). However, the adoption of the MSCKF VIN system in real-time embedded applications depends heavily on an efficient implementation of its tangled pipeline. This work initially proposes a novel parallel multi-thread implementation of the MSCKF VIN pipeline on an embedded CPU-enabled hardware that has speeded up the per-epoch processing time of the pipeline by 41% compared to the conventional sequential implementation. The heart of the MSCKF pipeline's visual backend is an inertially-aided 3D localization of visual feature points that are reduced to a set of nonlinear optimization problems which were conventionally solved in a serial fashion using the single-objective Gauss-Newton optimization algorithm. This work leveraged the parallel architecture of an embedded GPU and further proposes an efficient parallel implementation of a multi-objective Gauss-Newton algorithm. Integration of the proposed GPU-accelerated feature localization technique in the MSCKF parallel pipeline has resulted in 33% faster per-epoch processing time and consequently, the satisfaction of strict real-time constraints. The proposed parallel MSCKF VIN pipelines have been developed using C++ and CUDA on the NVIDIA Jetson TX2 embedded board. Experimental evaluations on a real visual-inertial odometry dataset have been provided to validate the efficacy and real-time performance enhancement of the proposed parallel implementation.