A Numerically Stable, Robust, and Efficient Minimal Solver for the Perspective-n-Points-Focal-Radial Problem

The Perspective-n-Points (PnPs) problem, which localizes a calibrated camera given 2-D–3-D point correspondences, is of fundamental importance in photogrammetry. To address zoom changes and radial distortion, it extends to the PnP-focal-radial (PnPfr) problem by adding estimates for focal length and radial distortion. Existing minimal solvers, designed for the four-point set-up and named P4Pfr methods, struggle to simultaneously perform well across numerical stability, noise robustness, and efficiency. Besides, they often involve solving geometrically infeasible solutions, increasing computational complexity, and generating redundant outputs. To this end, this work proposes a novel P4Pfr method with stable numeric, high robustness, and superior efficiency. Moreover, it can filter out invalid solutions during processing. The core is to find it on novel constraints based on point decentralization and pseudo depth factor. The point decentralization facilitates the numeric and robustness while the pseudo depth factor helps to select geometrically valid solutions on-the-fly. The proposed method simplifies the problem via the null space method and tackles the ordinary and planar configurations by Gröebner basis method and six-degree univariate polynomial, respectively. Synthetic and real-data experiments showcase that it exhibits the best or most competitive precisions in numerical stability and noise-robustness, with the highest efficiency and much lower outputs. Furthermore, thanks to these merits, the proposed method runs the fastest in random sample consensus (RANSAC) with comparable or better results.