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% .
Estimating a camera pose from 3D-2D correspondences - Perspective-n-Point - is one of the most fundamental problems in computer vision. Existing methods either solve a minimal problem or replace the objective with an easier one. Surprisingly, no scalable certifiable solver for the actual Maximum Likelihood Estimation (MLE) exists. We propose PnP*, the first practical solver that guarantees efficient computation and returns a certifiable globally optimal solution. We prove that the underlying convex relaxation is exact under moderate noise, which to our knowledge is the first stability guarantee for any relaxation for the MLE of PnP. Furthermore, we reduce the relaxation by exploiting chordal sparsity pattern and quotient ring reduction such that the relaxation is tractable and scalable. The stability and scalability results are verified numerically, as PnP* returns a provably correct camera pose at over 10 Hz.
Bang-Shien Chen, Caitrin E. Eaton, David M. Rosen
Northeastern University, Boston, MA 02115, USA. · Draper, Cambridge, MA 02139, USA.
This paper presents a new algorithm to estimate absolute camera pose given an axis of the camera's rotation matrix. Current algorithms solve the problem via algebraic solutions on limited input domains. This paper shows that the problem can be solved efficiently by finding the intersection points of a hyperbola and the unit circle. The solution can flexibly accommodate combinations of point and line features in minimal and overconstrained configurations. In addition, the two special cases of planar and minimal configurations are identified to yield simpler closed-form solutions. Extensive experiments validate the approach.
The blind perspective-n-point (PnP) problem is to estimate the pose of a calibrated camera from 2D and 3D point sets with unknown correspondences. We show that, for complete, noiseless observations, fourteen permutation-invariant image moments up to order four identify a fixed camera pose for almost every 3D point set containing at least twenty points. Our learning-free method, MomentPnP, matches these moments between the observed 2D points and the projected 3D points to initialize a camera pose without correspondences and refines the pose and correspondence weights using expectation--maximization. Across the test sets of three benchmarks, our method reduces median rotation error by 5--170× relative to the best learned baseline, both with and without outliers. We recover accurate poses even with as many outliers as inliers. On dense ETH3D scans, pose accuracy improves as the point sets grow, even as individual correspondences become harder to resolve.
Subhransu S. Bhattacharjee, Dylan Campbell, Rahul Shome
School of Computing, The Australian National University, Canberra