Stereo Visual Odometry with In-the-Loop Geometric Feed-Forward Models Towards Dense Metric Mapping
Authors: Fahira Afzal Maken, Sagun Man Singh Shrestha, Edward Vanderfeen, Lucas Carvalho de Lima, Rongxin Li, Changming Sun, Milad Ramezani
Organizations: CSIRO Robotics, Technology, CSIRO, Brisbane, Australia. · School of Electrical Engineering and Robotics, Queensland University of Technology (QUT), Brisbane, Australia.
We present a stereo visual odometry (SVO) framework that integrates geometric feed-forward models (GFMs) for dense depth prediction with factor-graph-based visual odometry to achieve accurate localisation and dense metric mapping. Our key idea is to exploit sparse but reliable metric geometry estimated from stereo and temporal multi-view observations to calibrate dense GFM predictions, while retaining the robustness and efficiency of model-based SVO. Rather than directly conditioning the GFM on potentially noisy stereo depth, optimised 3D landmarks are used to jointly estimate a scale-shift transformation over all keyframes within each local submap, yielding metrically consistent dense depth predictions. To balance accuracy and computational efficiency, we introduce a hierarchical factor-graph architecture. At the local level, sliding-window optimisation estimates high-frequency camera poses and multi-view-refined 3D landmarks. Dense predictions are generated only for selected keyframes and registered within each submap using SE(3) Generalised ICP. Consecutive submaps are further aligned in Sim(3) using their shared geometry, allowing residual scale inconsistencies to be corrected. These intra- and inter-submap constraints are incorporated into a global factor graph to produce a consistent trajectory and dense metric reconstruction. Extensive experiments on public and in-house datasets demonstrate that the proposed framework achieves competitive odometry accuracy against geometric and learning-based baselines while providing accurate dense metric reconstruction and improved robustness to failures in learned depth prediction.
Figures & tables
Fig. 1 : Top : stereo camera and LiDAR mounted on the in-house rig; the LiDAR is used only as reference. Middle : stereo feature tracking in the left and right images. Bottom : dense metric reconstruction of the in-house sequence, shown with submaps in distinct colours (left) and in RGB (right), viewed from the return leg along the corridor towards the start (green) and end (magenta) of the loop, together with the estimated (red) and reference (cyan) trajectories.
Fig. 2 : Factor structure of our SVO. Each LFG jointly optimises camera poses and 3 D landmarks using local stereo factors, while SE(3) ICP factors connect keyframes within the window of LFGs and Sim(3) factors across consecutive LFGs. The GFG maintains the overall trajectory using prior and inter-keyframe as well as between local window constraints. For simplicity, the superscript indicating the LFG count is omitted from m and X throughout the text.
Method
Int.
KITTI 01
KITTI 03
KITTI 05
KITTI 07
KITTI 09
NCD Short
trel
rrel
trel
rrel
trel
rrel
trel
rrel
trel
rrel
trel
rrel
ORB-SLAM3 [ 5 ]
5
0.15
0.05
0.08
0.07
0.08
0.21
0.10
0.14
0.09
0.08
0.81∗
2.45∗
10
0.31
0.08
0.15
0.10
0.12
0.24
0.14
0.20
0.17
0.11
1.62∗
2.65∗
50
1.48
0.26
0.61
0.23
0.44
0.43
0.41
0.44
0.70
0.34
6.93∗
3.24∗
100
2.69
0.35
1.11
0.30
0.79
0.59
0.69
0.60
1.23
0.48
10.90∗
3.72∗
DROID-SLAM [ 16 ]
5
5.28
0.05
0.08
0.08
0.10
0.22
0.40
0.66
0.09
0.11
2.65
2.80
TABLE I : Relative pose error (RPE) on KITTI odd-numbered sequences and NCD Short. Results are reported over trajectory intervals of 5, 10, 50, and 100 m . trel ( m ) and rrel ( \SIUnitSymbolDegree ) denote translational and rotational RPE, respectively. Lower is better.
Fig. 3 : Statistical analysis of translational (top) and rotational (bottom) RPE on the NCD Short Experiment. ∗ indicates the stereo visual odometry alone with the depth model disabled.
Fig. 4 : Comparison of the estimated trajectories on the NCD Short Experiment showing improved long-term trajectory consistency with dense-map constraints. ∗ indicates the stereo visual odometry alone with the depth model disabled.
Fig. 5 : Dense-map comparison on the in-house sequence: Ours (left), DROID-SLAM (middle), and ScaRF-SLAM (right). ScaRF-SLAM uses the poses estimated by our stereo VO, while DROID-SLAM uses its own front-end estimates; the estimated trajectory of each method are shown in blue. The reference LiDAR map is coloured according to its distance to each reconstruction: grey denotes surfaces within 10cm , while deviations increase from yellow at 10cm to red beyond 30cm . Regions without reconstructed surfaces therefore remain red, making the visualisation largely insensitive to reconstruction density. DROID-SLAM retains depth only for well-matched keyframes and consequently does not produce a comparably dense map.
Fig. 6 : Qualitative results on KITTI sequence 00 . Left: sparse metric landmarks estimated from local factor graphs. Middle: consecutively registered dense submaps. Right: estimated trajectory (cyan) and ground truth (red).
Method
ATE ↓
F-score ↑
Precision ↑
Recall ↑
Chamfer ↓
ScaRF-SLAM (ORB-SLAM3 poses)
0.074
55.2
70.6
45.3
0.192
ScaRF-SLAM (Our VO poses)
0.053†
54.1
71.9
43.4
0.193
DROID-SLAM (Front end)
0.055
54.6
90.2
39.1
0.179
Ours
0.053
64.9
68.2
61.9
0.162
TABLE II : Dense-map accuracy and ATE on the in-house run, all systems without loop closure. F-score, precision and recall at 10cm in %, ATE and Chamfer distance in m . All map scores follow one rigid alignment of the estimated trajectory to the reference.
Component
Laptop
AGX Orin
[ms/frame]
[ms/frame]
Front-end (feature tracking)
16.9
32.0
Local FGO
48.9
72.4
GFM inference
10.6
23.8
Metric depth alignment
2.1
5.4
Back-end (GICP + global FGO)
15.7
29.1
TABLE III : Average runtime per frame on the in-house run.
Deep learning-based visual odometry (VO) has achieved significant progress, yet most existing methods focus on a monocular approach, which suffers from scale ambiguity. Stereo VO provides real metric by its nature, but remains less studied in deep learning VO due to its high computational cost and modeling complexity. Recent advances in stereo matching and optical flow estimation have made dense visual correspondence increasingly accurate and reliable, but their complementary geometric information has not been fully exploited for VO. In this paper, we present SFVO, a correspondence-driven stereo VO framework that directly builds upon pretrained stereo matching and optical flow models. SFVO exploits pretrained stereo matching and optical flow models to estimate stereo and temporal correspondences. Instead of learning pose directly from images, SFVO maps learned correspondences into geometric constraints and predicts which points are trustworthy. To improve the reliability of visual correspondence-based geometric constraints, we introduce decoupled confidence maps for rotation and translation. This design better aligns the characteristics of visual correspondence and 6-DoF transformations. Extensive experiments on outdoor and indoor datasets demonstrate that SFVO achieves robust and accurate pose estimation with strong generalization capability. The code will be released.
Kai Zhang, Guoyang Zhao, Jun Ma
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
Dense metric mapping from a single camera and IMU remains challenging: visual-inertial odometry (VIO) provides metric motion but sparse geometry, whereas feed-forward depth models predict dense structure without ensuring metric scale. We present Accord, which constructs an accurate online dense metric map from any camera with an IMU. A VIO filter publishes metric poses, a depth foundation model conditioned on them predicts depth for short keyframe windows, and a factor graph fuses the resulting submaps. Recent work has explored several approaches to this problem. What is essential is the multi-view agreement (Accord), which is considered in SLAM as a source of geometric self-consistency. Accord anchors each keyframe window to metric 3D points triangulated from the filter poses, then exploits bidirectional dense depth agreement between co-visible images to estimate and correct per-image scale errors while preserving the metric scale established by the anchors. Context keyframes further improve depth prediction, and the resulting submaps are fused in a factor graph. We evaluate a single mapping configuration across five public camera-IMU setups and a new Phone-rig/GoPro dataset. With nothing tuned per dataset/setup and every map aligned only by Accord's own trajectory, Accord achieves consistently best/second-best mean F-score at 5 cm on every platform. Accord is open-source along with the phone-rig dataset for the benefit of the research community
Jaafar Mahmoud, Arthur Movsesyan, Cuong Huynh +3
Biomechatronics and Energy-Efficient Robotics (BE2R) Lab, ITMO University, Saint Petersburg, Russia
Feed-forward geometric foundation models (GFMs) directly predict dense geometry and camera poses from batches of images. Emerging visual SLAM methods chain together consecutive GFM batches to form dense point cloud maps. A core challenge is how to address the ambiguous geometric scale of GFM predictions: independent predictions are locally consistent up-to-scale but not constrained by any direct physical measurements. Models trained on data with known scale improve the consistency of GFM reconstructions but again do not directly measure it. Consequently, trajectories and 3D reconstructions from GFM-enabled SLAM methods inherit this issue. Motivated by parallels with classical visual-inertial odometry, we introduce FoundationVIO which integrates inertial sensing with GFM predictions to recover metric scale reconstruction using a factor-graph based approach. We demonstrate odometry estimates competitive with classical state-of-the-art VIO methods and accurate dense reconstructions that outperform GFM-enabled SLAM methods. We also add a minimal GFM-based loop-closure mechanism to create a full Visual-Inertial SLAM system capable of creating globally accurate reconstructions and pose estimates. We evaluate our method on three datasets with well known sensor systems and demonstrate its compatibility on ubiquitous camera and IMU sensors. The method runs at approximately real-time on a consumer-grade laptop GPU. Code: https://github.com/ori-drs/FoundationVIO.
Jin Rhee, Yuhao Zhang, Frank Dellaert +1
Oxford Robotics Institute, University of Oxford, U.K. · School of Interactive Computing, Georgia Institute of Technology, USA