An Efficient and Accurate Algorithm for the Perspecitve-n-Point Problem

In this paper, we address the problem of pose estimation from N 2D/3D point correspondences, known as the Perspective-n-Point (PnP) problem. Although many solutions have been proposed, it is hard to optimize both computational complexity and accuracy at the same time. In this paper, we propose an accurate and simultaneously efficient solution to the PnP problem. Previous PnP algorithms generally involve two sets of unknowns including the depth of each pixel and the pose of the camera. Our formulation does not involve the depth of each pixel. By introducing some intermediate variables, this formulation leads to a fourth degree polynomial cost function with 3 unknowns that only involves the rotation. In contrast to previous works, we do not address this minimization problem by solving the first-order optimality conditions using the off-the-shelf Gröbner basis method, as the Gröbner basis method may encounter numeric problems. Instead, we present a method based on linear system null space analysis to provide a robust initial estimation for a Newton iteration. Experimental results demonstrate that our algorithm is comparable to the start-of-the-art algorithms in terms of accuracy, and the speed of our algorithm is among the fastest algorithms.