Improving the computational efficiency of ROVIO

ROVIO is one of the state-of-the-art mono visual inertial odometry algorithms. It uses an Iterative Extended Kalman Filter (IEKF) to align features and update the vehicle state simultaneously by including the feature locations in the state vector of the IEKF. This algorithm is single core intensive, which allows using the other cores for other algorithms, such as object detection and path optimization. However, the computational cost of the algorithm grows rapidly with the total number of features. Each feature adds three new states (a 2D bearing vector and inverse depth), leading to bigger matrix multiplications which are computationally expensive. The main computational load of ROVIO is the iterative part of the IEKF. In this work, we reduce the average computational cost of ROVIO by 40% on an NVIDIA Jetson TX2, without affecting the accuracy of the algorithm. This computational gain is mainly achieved by utilizing the sparse matrices in ROVIO.