Active online visual-inertial navigation and sensor calibration via belief space planning and factor graph based incremental smoothing

High accuracy navigation in GPS-deprived environments is of prime importance to various robotics applications and has been extensively investigated in the last two decades. Recent approaches have shown that incorporating sensor's calibration states in addition to the 6DOF pose states may cause better performance of the system. However, these approaches typically consider a passive setting, where robot actions are externally defined. On the other hand, belief space planning (BSP) approaches account for different sources of uncertainty, thus identifying actions that improve certain aspects in inference, such as accuracy. Yet, existing BSP approaches typically do not consider sensor calibration, nor a visual-inertial SLAM setup. In this paper we contribute a BSP approach for active sensor calibration of a visual-inertial SLAM setup. For this purpose we incorporate within the belief both robot's pose and sensor calibration states while considering operation in partially unknown and uncertain environment. In particular, we leverage the recently developed concept of IMU pre-integration and develop appropriate factor graph formulation for future beliefs to facilitate computationally efficient inference within BSP. Our approach is valid for general cost functions, and can be used to identify best robot actions from a given set of candidate actions or to calculate locally-optimal actions using direct trajectory optimization techniques. We demonstrate our approach in high-fidelity synthetic simulation and show that incorporate sensors calibration state into the BSP significantly improved estimation accuracy.