Efficient Solution to PnP Problem Based on Vision Geometry

Perspective-n-Point (PnP) problem aims to estimate pose from known 3D map points and their projections. Efficient PnP (EPnP), one of the classical PnP solvers, represents camera pose with control points, which are easier to estimate utilizing the least square (LS) formulation. However, the geometry refinement procedure performed by most EPnP-based methods is separated from the solution of LS formulation, which creates difficulty in balancing between minimizing the loss function and preserving the essential geometry properties of control points. To handle the problem, we propose a novel method to integrate geometry constraints into control points formulation and reformulate the LS to quadratic constraints quadratic programming (QCQP). We deduce an innovative analytical solution, dubbed ACEPnP, to the constrained EPnP problem, which is faster than the customarily applied numerical methods. An uncertainty-aware least square registration procedure is designed in ACEPnP to compute camera pose from control points. Our method is plug-and-play and can be embedded in various variants of EPnP. Experiments in synthetic and real data show that our methods outperform other state-of-the-arts. Compared to the best PnP solver, EPnPU*, our approaches reduce rotation errors by 12.2% and translation errors by 34.8% with similar time consumption on the KITTI odometry dataset.