Organizations: School of Data Science, Chinese University of Hong Kong, Shenzhen 518172, China. · Australian Center for Robotics and School of Aerospace, Mechanical and Mechatronic Engineering, University of Sydney, Australia.
In this paper, we first present a bias-eliminated weighted (Bias-Eli-W) perspective-n-point (PnP) estimator for stereo visual odometry (VO) with provable consistency. Specifically, leveraging statistical theory, we develop an asymptotically unbiased and n-consistent PnP estimator that accounts for varying 3D triangulation uncertainties, ensuring that the relative pose estimate converges to the ground truth as the number of features increases. Next, on the stereo VO pipeline side, we propose a framework that continuously triangulates contemporary features for tracking new frames, effectively decoupling temporal dependencies between pose and 3D point errors. We integrate the Bias-Eli-W PnP estimator into the proposed stereo VO pipeline, creating a synergistic effect that enhances the suppression of pose estimation errors. We validate the performance of our method on the KITTI and Oxford RobotCar datasets. Experimental results demonstrate that our method: 1) achieves significant improvements in both relative pose error and absolute trajectory error in large-scale environments; 2) provides reliable localization under erratic and unpredictable robot motions. The successful implementation of the Bias-Eli-W PnP in stereo VO indicates the importance of information screening in robotic estimation tasks with high-uncertainty measurements, shedding light on diverse applications where PnP is a key ingredient.
Figures & tables
Figure 1: System overview. L and R refer to the left and right images, respectively, and KF and CF denote the keyframe and current frame, respectively.
Figure 2: Illustration of frame tracking. The orange circles represent feature-matching uncertainties and the blue ellipses denote the triangulation uncertainties.
Figure 3: Illustration of epipolar BA. For two KFs, we utilize stereo images, while for OFs, we only use the left image. Epipolar constraints for each pair of images with matched features are involved in BA, and point-to-epipolar-line distances are optimized.
Figure 4: Consistency of the bias-eliminated PnP estimator. The units of the RMSEs of noise std, rotation, and translation are pixel, ∘ , and m.
Trajectory
KF used
ATE
RPE
t
R
t
R
Line
latest
1.161
2.452
0.046
0.048
two
2.193
0.947
0.176
0.175
three
2.858
1.859
0.170
0.163
latest+BA
1.068
1.307
0.044
0.057
Table I : Average errors of using different numbers of KFs for PnP tracking. The units for errors of t and R are m and ∘ .
Figure 5: Average pose error of each frame when using different numbers of KFs for the line trajectory.
Sequence
ATE (m)
RPE (m)
Color
Grayscale
Color
Grayscale
ORB3
OV2
Ours
ORB3
OV2
Ours
ORB3
OV2
Ours
ORB3
OV2
Ours
seq00
4.042
4.676
4.174
4.263
4.767
4.514
0.0287
0.0278
0.0262
0.0283
0.0262
0.0260
seq02
9.549
11.406
5.756
7.900
7.363
3.900
0.0286
0.0278
0.0257
0.0277
0.0263
0.0257
seq03
3.846
4.183
0.551
1.200
1.177
1.030
0.0250
0.0264
0.0148
0.0182
0.0166
0.0158
Table II: Comparison of ATE and RPE across different sequences in KITTI dataset. ORB3 denotes ORB-SLAM3 and OV2 represents OV 2 SLAM. Values highlighted in blue bold represent the smallest, and values in blue denote the second smallest.
Figure 6: Top-view trajectory of KITTI sequences on grayscale images.
Sequence
Start frame
Stop frame
Length (m)
ATE (m)
RPE (m)
ORB3
OV2
Ours
ORB3
OV2
Ours
2014-05-14-13-59-05
1400075963932033
1400076150344330
945.49
34.5311
34.6471
33.7454
0.5126
0.5146
0.5061
2014-05-19-12-51-39
1400503987511809
1400504194609323
931.58
-
28.9658
28.3328
-
0.4761
0.4682
2014-05-19-13-20-57
1400505700597042
1400506098919265
1989.92
-
72.4767
64.9658
-
0.5768
0.5680
2014-11-14-16-34-33
1415985043842007
1415985331240621
1531.52
24.1157
15.5798
6.2296
0.5773
0.5872
0.5674
Table III: Comparison of ATE and RPE across different sequences in Oxford dataset. Values highlighted in blue bold represent the smallest, and values in blue denote the second smallest. The sign - indicates failing to estimate the whole trajectory.
Figure 7: Error analysis on a challenging segment with varying velocity. The figure consists of four key components: the left are sequential images with reprojection results, the middle top shows the bird’s-eye view of the trajectory with a zoomed section, the right top presents a detailed visualization of camera motion with velocity annotations, and the bottom plots quantitative errors.
Robotics and Autonomous Systems Thrust, The Hong Kong University of Science and Technology (Guangzhou), Guangzhou 511453, China · Cheng Kar-Shun Robotics Institute, The Hong Kong University of Science and Technology, Hong Kong SAR, China