Fast certifiable relative pose estimation with gravity prior

Abstract

Redundant and complementary information from different types of sensors boosts the robustness of autonomous systems, making them more reliable and safer. In particular, inertial measurement units (IMUs) are increasingly being integrated with cameras for that purpose, since the information provided by the IMU helps to simplify some visual problems and improves the accuracy of the results. In the context of estimating the motion of a camera, which is the problem we address in this work, the gravity vector delivers by the IMU reduces the unknown rotation to only one degree of freedom instead of three, hence simplifying the relative pose problem (RPp). Despite this simplification, the RPp is still nonconvex, therefore the quality (optimality) of the solution returned by iterative solvers cannot be guaranteed. These suboptimal solutions may have serious consequences for applications that have this solver as a key block, and may even cause their complete failure.In this paper, we contribute a certifiable solver for the RPp with gravity prior. We propose an iterative certifier that does not assume any condition on the problem, and returns an optimality certification even for an overconstrained formulation with 28 constraints in less than 1.5 milliseconds. Since the certifier doesn't obtain the solution to the problem, we also provide a fast, iterative on-manifold estimation of the relative pose, which is shown to return solutions with lower costs than other nonminimal solvers in less time. We make the code available at https://www.github.com/mergarsal.