Organizations: School of Engineering & Physical Sciences, Heriot-Watt University, Edinburgh, UK. · Engineering Department, Japan Agency for Marine-Earth Science and Technology, Yokosuka, Japan.
Accurate state estimation for autonomous underwater vehicles performing close-range seafloor sampling remains challenging. In low-altitude operation, down-looking cameras over featureless planar seabeds produce scale ambiguity, lateral degeneracy, and inconsistent feature tracking. Meanwhile, inertial-Doppler Velocity Log (DVL) fusion alone provides no mechanism for structural drift correction. We propose a Contact-Aided Factor-Graph Localization framework that treats physical interaction as an informative geometric constraint within a smoothing-based localization formulation. The method tightly fuses suction-based manipulator contact events with adaptive visual odometry, learned object detections, and on-board sensors. Visual odometry relative-pose factors and landmark bearing-range factors are uncertainty-scaled according to inlier statistics to prevent visually weak frames from destabilizing the estimator, while contact events are modeled as high-confidence factors that induce implicit loop closures without appearance-based place recognition. Furthermore, the system can fully initialize online during motion. Experimental evaluation in tanks, harbor, and simulation environments demonstrates that contact-induced constraints significantly reduce trajectory drift and improve object revisit accuracy compared to filtering-based navigation and contact-free graph formulations. These results highlight the role of embodied physical interaction as a localization primitive in perception-degraded underwater environments
Figures & tables
Fig. 1 : Suction-based implicit loop closure. (a) Visual–inertial odometry accumulates drift over time despite continuous visual and inertial constraints. (b) When the manipulator establishes suction contact, the contact point is modeled as a fixed landmark, anchoring the current pose with a high-confidence constraint. (c) This contact induces an implicit loop closure in the factor graph, propagating corrections through the trajectory and reducing global drift.
Fig. 2 : Experimental environments. Top: first tank (low light), second tank (natural sunlight), and harbor deployment site. Bottom: representative best-match image pairs illustrating the geometric degeneracy and impracticality of classical Structure-from-Motion and monocular visual SLAM in these conditions.
Fig. 3 : First tank experiments : (a) T1: straight trajectory; (b) T2: two 180 ∘ rotations; (c) T3: rotations with starfish landmark; (d) T4: rotations with depth variation; (e) T5: depth variation and starfish landmark. Red markers denote the starfish target, and dashed green lines indicate segments with improved estimation accuracy.
TABLE I : Trajectory accuracy metrics from the first tank experiments. The upper section reports full-system performance across different motion profiles. The lower section presents the ablation study conducted on Trajectory T1 (No Rotation).
Fig. 4 : Stonefish simulation experiments. Estimated trajectories using the proposed FG-UCAL framework are compared against ground-truth (perfect odometry) for representative motion patterns: S1: forward–lateral motion, S2: multiple rotations, S3: continuous rotations, S4: depth with yaw variation, and a S5: semi-8 trajectory with two starfish dives.
Trajectory
ATE (m)
RPE (m)
Yaw RMSE (rad)
Yaw RPE (rad)
S1: Forward and lateral movement
0.0903
0.0039
0.0538
0.0039
S2: Multiple rotations
0.1635
0.0060
0.0315
0.0025
S3: Continuous rotations
0.998
0.0032
0.0886
0.0415
S4: Depth + yaw change
0.0847
0.0052
0.0981
0.0080
S5: Semi 8 shape + 2 starfish dives
0.0847
0.0052
0.0981
0.0080
↪ Landmark and Pump – Starfish revisit distance (m)
0.06 m
TABLE II : Trajectory error metrics in Stonefish simulation relative to ground-truth odometry. ATE and RPE are reported in meters; angular errors in radians.
Fig. 5 : Sequence showing the robot detecting the starfish, approaching it and use the suction-cup to interact with it.
Fig. 6 : a) Second tank experiments : left) ST1: 2 starfish - 2 pump activation on the last, center) ST2: 2 starfish - 2 pump activation on the first, right) ST3: 1 starfish - 2 pump activations. b) Harbor experiment , employing semi-circular trajectory plus 2 dives. Red and gold star markers denote two different starfish targets. Two red stars indicate two separate dives starting from different locations; Cyan triangles indicate suction-pump activation events, and dashed green lines highlight trajectory segments with improved estimation accuracy.
Trajectory
ATE (m)
RPE (m)
Yaw RMSE (rad)
Yaw RPE (rad)
Second Tank Experiments
ST1: 2 starfish – 2 pump activation (last)
1.0158
0.0035
0.1836
0.0059
ST2: 2 starfish – 2 pump activation (first)
0.4985
0.0064
0.2935
0.0303
ST3: 1 starfish – 2 pump activation
0.4464
0.0152
0.1411
0.0045
↪ No landmark – Starfish revisit distance (m)
0.58m
↪ Landmark – Starfish revisit distance (m)
0.50m
TABLE III : Trajectory accuracy metrics for the Second Tank and Harbor experiments using the full system configuration. Metrics include Absolute Trajectory Error (ATE), Relative Pose Error (RPE), Yaw RMSE, and Yaw RPE.
This paper presents a novel acoustic-visual-inertial odometry solution leveraging a continuous-time trajectory estimation framework for unmanned underwater vehicles. Underwater environments present unique challenges for visual localization and mapping, such as light attenuation, illumination variance, and the presence of particulate matter. This motivates the use of additional sensing modalities and a visual tracking pipeline that is robust to diverse subsea conditions. The proposed system is the first continuous-time trajectory estimation framework based on Gaussian processes to fuse asynchronous measurements from a Doppler velocity log, a stereo camera, and an inertial measurement unit. Additionally, a novel visual frontend is proposed, incorporating learning-based feature extraction and matching that is robust to the specific challenges that subsea environments present. The proposed framework enables seamless integration of additional sensor modalities in continuous-time and is adaptable to different environments without reconfiguration. The proposed system is extensively tested on real-world underwater inspection datasets, where it outperforms state-of-the-art visual-inertial and acoustic-visual-inertial SLAM algorithms in accuracy, robustness, and trajectory coverage. Notably, the proposed system outperforms the state-of-the-art despite only forming short-term visual data associations.
Kyungmin Jung, Angad Bajwa, Junha Yoo +2
Department of Mechanical Engineering, McGill University, Montreal, QC, Canada
Remotely operated vehicles (ROVs) are widely used to explore and inspect underwater environments such as caves, shipwrecks, and submerged infrastructure. These missions require accurate 3D understanding of the surrounding environment, which depends on both reliable vehicle localization and metric scene reconstruction. However, external positioning is often unavailable underwater, requiring small ROVs to rely primarily on onboard perception. Optic vision provides rich visual and geometric information but suffers from scale ambi- guity and trajectory drift, whereas 2D imaging sonar provides metric range but incomplete 3D geometry. Existing underwater reconstruction approaches typically address these limitations separately or assume known sensor poses, leaving localization and reconstruction disconnected. We present SURGE, a camera sonar framework that jointly estimates the ROV trajectory and target location by integrating visual and acoustic observations within a factor graph, then uses the recovered metric poses for sonar Gaussian splatting. Experiments on real underwater RGB sonar observations show that SURGE substantially improves localization consistency over conventional vision based pose estimation and produces a more compact, natively metric reconstruction than RGB Gaussian splatting baselines.
Mohammed Ibrahim M, Vallabh Deogaonkar, Trung Dong +3
Department of Electrical and Computer Engineering University of South Florida Tampa, FL, USA · Department of Ocean Engineering Indian Institute of Technology Madras Chennai, India · Department of Naval Architecture and Ocean Engineering Seoul National University Seoul, South Korea
Extreme subsea environments often cause severe feature de-gradation and estimator divergence in underwater visual-inertial SLAM. Although sensors like Doppler Velocity Logs (DVL) and pressure gauges provide auxiliary constraints, robust multi-sensor fusion during intermittent visual failure remains challenging. To address this, we present APVI-SLAM, a real-time multi-sensor fusion SLAM system that achieves both accurate underwater localization and photorealistic mapping. Our approach introduces a reliability-aware localization framework that dynamically reweights sensor estimators and employs a sliding-window freezing strategy to recover from tracking failures, substantially enhancing system robustness. Furthermore, for high-fidelity scenes reconstruction, we propose an efficient quadtree-guided mapping module that facilitates incremental water-medium modeling and 3D Gaussian optimization. Recognizing the lack of benchmark for underwater mapping evaluation, we also contribute a coral reef surveying dataset with synchronized multi-modality data. Extensive experiments on public and our proposed benchmarks demonstrate that APVI-SLAM achieves state-of-the-art localization and reconstruction quality at real-time speeds.
Hanwen Zhang, Yipeng Zhu, Xiaopeng Guo +2
Division of Integrative Systems and Design, Hong Kong University of Science and Technology · School of Interdisciplinary Science, Beijing Institute of Technology · Division of Integrative Systems and Design, the Department of Computer Science and Engineering, and the Department of Ocean Science, Hong Kong University of Science and Technology