A consistently fast and accurate algorithm for estimating camera pose from point correspondences

This paper presents an accurate and simultaneously efficient algorithm for the Perspective-n-Point (PnP) problem that estimates the absolute pose of a fully-calibrated camera from given 3D-to-2D point correspondences. Previous works typically use the depth of each 3D point to formulate the PnP problem, which will bring extra variables. In contrast, the presented algorithm does not involve any depth factor. By introducing the Cayley–Gibbs–Rodriguez (CGR) parameterization, the modified formulation is first compressed to a nonlinear least-square cost function that only depends on three unknown rotation parameters and a known symmetric coefficient matrix. The Gröbner basis method is then adopted to find a set of solutions for the rotation parameters by solving a third-order polynomial system arising from the first-order optimality conditions of the cost function. Lastly, the rotation and translation are efficiently computed by back-substitution. Furthermore, a novel approach is developed for handling singularities of the CGR parameterization. It is improved by applying fixed pre-rotations, as opposed to randomly generated rotations in previous works, to 3D points. This improvement will facilitate the calculation of the coefficient matrix involved in a cost function when re-solving the PnP problem. Extensive experiments on both simulated and real data demonstrate that the presented algorithm can achieve the state-of-the-art accuracy with reduced computational requirements.