A simple, robust and fast method for the perspective-n-point Problem

In this work, we present a simple, robust and fast method to the perspective-n-point (PnP) problem for determining the position and orientation of a calibrated camera from known reference points. Our method transfers the pose estimation problem into an optimal problem, and only needs to solve a seventh-order and a fourth-order univariate polynomial, respectively, which makes the processes more easily understood and significantly improves the performance. Additionally, the number of solutions of the proposed method is substantially smaller than existing methods. Experiment results show that the proposed method can stably handle all 3D point configurations, including the ordinary 3D case, the quasi-singular case, and the planar case, and it offers accuracy comparable or better than that of the state-of-art methods, but at much lower computational cost.