Element-wise weighted total least square for monocular pose estimation

Monocular pose estimation is widely used for guidance, navigation, and control of spacecraft, Unmanned Aerial Vehicles (UAVs), and autonomous vehicles. Although hybrid pipelines that couple handcrafted feature extraction with Perspective-n-Point (PnP) solvers remain the mainstream solution, current PnP algorithms face two critical limitations: most are tailored to the pinhole camera model, leading to degraded accuracy on fisheye or panoramic images; and few methods jointly model the uncertainties of both 2-D image measurements and 3-D object points, which introduces estimation bias. To overcome these challenges, this paper presents an Element-wise Weighted Total Least Squares (EWTLS) approach. Specifically, a unified spherical camera model is introduced to accommodate wide Field-Of-View (FOV) lenses. Each correspondence is weighted by its associated 2-D and 3-D covariance. Assuming small perturbations, closed-form expressions are derived, which coincide with the Cramér-Rao Lower Bound (CRLB), thereby demonstrating first-order efficiency and optimality. Extensive Monte Carlo simulations confirm the consistency of the theoretical covariance expressions, and experiments on both synthetic and real-world datasets show that the proposed algorithm outperforms state-of-the-art methods.