FUSE-PnPL: Flexible, Unified, Statistically Consistent, and Asymptotically Efficient Camera Pose Estimation from Points and Lines
Guangyang Zeng, Yuan Shen, Yiguo Peng, Guodong Shi, Biqiang Mu, Junfeng Wu
原文摘要
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.
cs.RO