Certifiably Optimal and Robust Camera Pose Estimation From Points and Lines

The Perspective-n-Point-and-Line (PnPL) problem, as a cornerstone in geometric computer vision, seeks to estimate the absolute pose of a calibrated camera from 3D-to-2D point and line correspondences. In this paper, we present a certifiably globally optimal and robust solution to the PnPL problem with a large number of outliers among the correspondences. Our first contribution is to reformulate the general PnPL problem as a novel constrained global optimization problem of Sum-of-Squares (SOS) polynomials using generalized geometric distances for both points and lines. Our second contribution is to efficiently solve this non-convex optimization problem by reducing it to an equivalent convex Linear Matrix Inequality (LMI) problem via a series of SOS relaxations. With these two contributions, we can develop a non-minimal solver, named SPnPL, for the outlier-free cases. The third contribution is to further add a Graduated Non-Convexity (GNC) cost function to SPnPL so as to remove outliers through closed-form iterations, which leads to a robust solver, named GNC-SPnPL. Both synthetic and real-data experiments confirm that SPnPL can mostly outperform the existing state-of-the-art PnPL algorithms and GNC-SPnPL can render accurate results even when 85% of the correspondences are outliers.