cs.ROJul 23, 2024

FUSE-PnPL: Flexible, Unified, Statistically Consistent, and Asymptotically Efficient Camera Pose Estimation from Points and Lines

Authors: Guangyang Zeng, Yuan Shen, Yiguo Peng, Guodong Shi, Biqiang Mu, Junfeng Wu

Organizations: School of Data Science, Chinese University of Hong Kong, Shenzhen, Shenzhen, China · Australian Center for Field Robotics, School of Aerospace, Mechanical and Mechatronic Engineering, The University of Sydney, Sydney, Australia · Key Laboratory of Systems and Control, Institute of Systems Science, Academy of Mathematics and Systems Science, Chinese Academy of Sciences, Beijing, China

Abstract

Camera pose estimation from 3D-2D correspondences is fundamental to robotics. Three practical considerations motivate this work. First, point and line features provide complementary geometric information, motivating a unified estimator. Second, the number and composition of available features vary across scenarios, requiring support for minimal and arbitrary non-minimal configurations. Third, modern visual front ends extract abundant features from individual images, enabling highly accurate pose estimation. We propose FUSE-PnPL, a flexible and unified camera pose estimator that is statistically consistent and asymptotically efficient. Specifically, we represent each line using two distinct points and adopt a unified image-space noise model for point and line measurements. By employing Plücker coordinates for line parameterization, we explicitly characterize the propagation of image endpoint noise into line residuals and formulate a joint maximum likelihood (ML) estimation problem for combined point and line measurements. To efficiently solve this problem, FUSE-PnPL employs a two-stage estimation scheme. First, noise variance estimation and bias elimination yield a consistent initial pose estimate. A single Gauss-Newton iteration then refines this estimate, achieving asymptotic efficiency as its mean squared error asymptotically attains the Cramér-Rao lower bound. Moreover, the proposed estimator has linear computational complexity with respect to the number of correspondences, making it suitable for high-precision and real-time robot pose estimation. Extensive experiments on synthetic data and real-world benchmarks validate the theoretical properties of FUSE-PnPL and demonstrate its advantages in both static global localization and dynamic SLAM systems.

Figures & tables

Explore similar work

CardsList
  1. PnP*: A certifiable solver for Perspective-n-Point

    Oct 22, 2026Bang-Shien Chen, Caitrin E. Eaton, David M. RosenCamera Pose EstimationConvex Relaxation

  2. Correspondence-Free Camera Pose Estimation by Matching Moments

    Nov 16, 2026Subhransu S. Bhattacharjee, Dylan Campbell, Rahul ShomeCamera Pose Estimationperspective-n-point pose estimation