Camera Pose and Focal Length Estimation Using Regularized Distance Constraints

Camera pose and intrinsic parameters estimation from n 2D-to-3D point correspondences is a known problem in computer vision and photogrammetry. Depending on the set of unknown parameters, the problem is called Perspective-n-Point (PnP) when only absolute camera pose is unknown or PnPf when focal length is unknown as well. Projection error functions are highly non-convex in focal length, so before methods for PnPf were published, the only choice was to do exhaustive search not suitable for real-time applications. The EPnP method was extended to PnPf problem in [4], we refer to this method as UPnPf. RPnP inspired the authors of [5] to propose a method GPnPf+GN for PnPf problem. They use angle constraints to build specific polynomial system and solve it, then they use nonlinear refinement with Gauss-Newton algorithm. It gave superior results to [4] both in speed and accuracy in general case, and was more accurate in planar case, although UPnPf [4] was faster. This paper is devoted to a method for PnPf problem for arbitrary amount of points, more or equal to 6. We consider both planar and nonplanar cases. We fix the space of the search as a linear combination of several right singular vectors of the least squares system matrix. We use linear programming techniques to find feasible solutions faster. Then we do nonlinear refinement with Levenberg-Marquardt. The barycentric representation of 3D points allows to express n 3D points pi as a frame-independent linear combination of 4 basis points c j: