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
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
Fig. 1: Large sample features extracted from a single image.
Fig. 2: Illustration of point and line correspondences.
Fig. 3: Overview of algorithm framework.
Fig. 4: MSEs of noise variance estimates.
Fig. 5: MSEs of PnP pose estimates.
Fig. 6: MSEs versus GN iteration number under a challenging configuration.
Fig. 7: MSEs of PnL pose estimates.
Fig. 8: MSEs of PnPL pose estimates.
Fig. 9: MSEs of different kinds of measurements.
Fig. 10: Accumulated CPU time comparison over 1000 Monte Carlo tests.
Fig. 11: Ablation study of bias elimination under a challenging configuration.
Fig. 12: Mean PnP pose estimation error comparison in 13 scenes of the ETH3D dataset.
Fig. 13: Mean PnL pose estimation error comparison in 7 scenes of the VGG dataset.
Fig. 14: Mean PnPL pose estimation error comparison in 7 scenes of the VGG dataset.
Fig. 15: Line reprojection onto images with estimated poses.
Case
Method
Noise-free
1 px
CR
MSE ( R )
MSE ( t )
P3P
FUSE-PnP
100%
0.9816
16.2495
DLS
100%
2.3140
33.5204
SQPnP
96%
3.0056
44.9912
P3L
FUSE-PnL
100%
0.8724
11.0975
ASPnL
100%
3.4731
33.9313
TABLE I: Minimal-case tests. CR denotes the coverage rate of the ground-truth pose by the candidate set.
Sequence
AirSLAM
AirSLAM(ours)
ATE
RPE ( t )
RPE ( R )
ATE
RPE ( t )
RPE ( R )
MH_01_easy
0.0879
0.0228
0.235
0.1002 (+13.99%)
0.0213 (-6.69%)
0.224 (-4.53%)
MH_02_easy
0.0941
0.0169
0.189
0.0908 (-3.49%)
0.0159 (-5.75%)
0.183 (-3.25%)
MH_03_medium
0.1439
0.0326
0.253
0.1135 (-21.11%)
0.0317 (-2.95%)
0.235 (-7.06%)
MH_04_difficult
0.3794
0.0404
0.232
0.2847 (-24.96%)
0.0391 (-3.32%)
0.227 (-2.09%)
MH_05_difficult
0.2416
0.0343
0.224
0.2115 (-12.45%)
0.0332 (-3.04%)
0.221 (-1.10%)
TABLE II: AirSLAM comparison on the EuRoC MAV dataset. ATE and translation RPE are in meters, while rotation RPE is in degrees. Boldface indicates lower errors, and parentheses show percentage changes relative to the original AirSLAM.
Sequence
PL-SLAM
PL-SLAM(ours)
ATE
RPE ( t )
RPE ( R )
ATE
RPE ( t )
RPE ( R )
MH_01_easy
0.9424
0.0440
0.313
0.9439 (+0.16%)
0.0435 (-1.19%)
0.314 (+0.33%)
MH_02_easy
0.7817
0.0339
0.360
0.7807 (-0.13%)
0.0328 (-3.35%)
0.345 (-4.35%)
MH_03_medium
1.3044
0.0533
0.480
1.3058 (+0.11%)
0.0535 (+0.36%)
0.485 (+1.04%)
MH_04_difficult
2.2395
0.1665
0.911
2.2190 (-0.91%)
0.1363 (-18.11%)
0.802 (-12.00%)
MH_05_difficult
1.8837
0.1525
0.890
1.8648 (-1.00%)
0.1293 (-15.22%)
0.792 (-10.99%)
TABLE III: PL-SLAM comparison on the EuRoC MAV dataset.
Fig. 16: Experimental setup and SLAM result. Top: Pool environment and captured scene images. Bottom: Underwater robot, stereo camera, and a SLAM result example.
Sequence
AirSLAM
AirSLAM(ours)
ATE
RPE ( t )
RPE ( R )
ATE
RPE ( t )
RPE ( R )
PS-E01
0.1033
0.1010
4.182
0.0991 (-4.05%)
0.0999 (-1.05%)
4.141 (-1.00%)
PS-E02
0.0660
0.0748
2.790
0.0627 (-4.97%)
0.0734 (-1.85%)
2.716 (-2.64%)
PS-E03
0.0671
0.0847
2.366
0.0559 (-16.69%)
0.0797 (-5.92%)
2.284 (-3.46%)
PS-M01
0.1221
0.0715
2.632
0.0718 (-41.13%)
0.0714 (-0.14%)
2.629 (-0.12%)
PS-M02
0.1097
0.1086
5.527
0.0862 (-21.38%)
0.1047 (-3.62%)
5.251 (-4.99%)
TABLE IV: AirSLAM comparison on our self-collected PoolStereo dataset.
Sequence
PL-SLAM
PL-SLAM(ours)
ATE
RPE ( t )
RPE ( R )
ATE
RPE ( t )
RPE ( R )
PS-E01
0.2147
0.1078
4.480
0.1131 (-47.33%)
0.1042 (-3.35%)
4.297 (-4.07%)
PS-E02
0.2677
0.0724
2.451
0.1169 (-56.33%)
0.0721 (-0.40%)
2.533 (+3.36%)
PS-E03
0.1574
0.0898
2.732
0.1436 (-8.77%)
0.0833 (-7.24%)
2.579 (-5.59%)
PS-M01
0.1761
0.0746
2.844
0.1457 (-17.22%)
0.0733 (-1.80%)
2.783 (-2.15%)
PS-M02
0.1939
0.1128
5.655
0.1629 (-16.00%)
0.1132 (+0.37%)
5.712 (+1.01%)
TABLE V: PL-SLAM comparison on our self-collected PoolStereo dataset.
Fig. 17: Odometry results on the EuRoC MAV and PoolStereo datasets. From left to right: trajectory comparisons for AirSLAM and PL-SLAM on the EuRoC MAV dataset and the PoolStereo dataset, respectively. From top to bottom: easy, medium, and difficult sequences. Black dotted lines denote reference trajectories.
Dataset
Pipeline
Δ ATE
Δ RPE ( t )
Δ RPE ( R )
EuRoC
AirSLAM
+1.56
+0.45
+0.96
PL-SLAM
−0.22
−0.01
−0.02
PoolStereo
AirSLAM
+8.33
+0.31
+0.24
PL-SLAM
+0.54
+0.01
+0.13
TABLE VI: Ablation study of bias elimination in FUSE-PnPL . Relative changes in sequence-averaged RMSEs ( Eˉ ) after disabling bias elimination: Δ=(Eˉoff/Eˉon−1)×100% .