A Stable Algebraic Camera Pose Estimation for Minimal Configurations of 2D/3D Point and Line Correspondences

摘要

This paper proposes an algebraic solution for the problem of camera pose estimation using the minimal configurations of 2D/3D point and line correspondences, including three point correspondences, two point and one line correspondences, one point and two line correspondences, and three line correspondences. In contrast to the previous works that address these problems in specific geometric ways, this paper shows that the above four cases can be solved in a generic algebraic-framework. Specifically, the orientation of the camera is computed from a polynomial equation system of four quadrics, then the translation can be solved from a linear equation system. To make our algorithm stable, the key is the polynomial solver. We significantly improve the numerical stability of the efficient three quadratic equation system solver, E3Q3 [17], with a slight computational cost. The simulation results show that the numerical stability of our algorithm is comparable to the state-of-the-art Perspective-3-Point (P3P) algorithm [14], and outperforms the state-of-the-art algorithms of the other three cases. The numerical stability of our algorithm can be further improved by a rough estimation of the rotation matrix, which is generally available in the Localization and Mapping (SLAM) or Visual Odometry (VO) system (such as the pose from the last frame). Besides, this algorithm is applicable to real-time applications.