Cooperative visual-inertial sensor fusion: fundamental equations and state determination in closed-form

Abstract

This paper investigates the visual and inertial sensor fusion problem in the cooperative case and provides new theoretical and basic results. Specifically, the case of two agents is investigated. Each agent is equipped with inertial sensors (accelerometer and gyroscope) and with a monocular camera. By using the monocular camera, each agent can observe the other agent. No additional camera observations (e.g., of external point features in the environment) are considered. First, the entire observable state is analytically derived. This state contains the relative position between the two agents (which includes the absolute scale), the relative velocity, the three Euler angles that express the rotation between the two local frames and all the accelerometer and gyroscope biases. Then, the basic equations that describe this system are analytically obtained. The last part of the paper describes the use of these equations to obtain a closed-form solution that provides the observable state in terms of the visual and inertial measurements provided in a short time interval. This last contribution is the extension of the results presented in Kaiser et al. (IEEE Robot Autom Lett 2(1):18–25, 2017), Martinelli (IEEE Trans Robot 28(1):44–60, 2012; Int J Comput Vis 106(2):138–152, 2014) to the cooperative case. The impact of the presence of the bias on the performance of this closed-form solution is also investigated and a simple and effective method to obtain the gyroscope bias is proposed. Extensive simulations clearly show that the proposed method is successful. It is worth noting that it is possible to automatically retrieve the absolute scale and simultaneously calibrate the gyroscopes not only without any prior knowledge (as in Kaiser et al. IEEE Robot Autom Lett 2(1):18–25, 2017), but also without external point features in the environment.