Contact-Aided Factor-Graph Localization for Underwater Sampling

Michele Grimaldi; Yosaku Maeda; Hitoshi Kakami; Ignacio Carlucho; Yvan R. Petillot; Tomoya Inoue

cs.ROAug 27, 2026

Contact-Aided Factor-Graph Localization for Underwater Sampling

Authors: Michele Grimaldi, Yosaku Maeda, Hitoshi Kakami, Ignacio Carlucho, Yvan R. Petillot, Tomoya Inoue

Organizations: School of Engineering & Physical Sciences, Heriot-Watt University, Edinburgh, UK. · Engineering Department, Japan Agency for Marine-Earth Science and Technology, Yokosuka, Japan.

Abstract

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.

Full System – Different Motion Profiles
Configuration / Trajectory	ATE (m)	RPE (m)	Yaw RMSE (rad)	Yaw RPE (rad)
T1: No Rotation	0.0990	0.0812	0.0524	0.0113
T2: $2\times 180^{\circ}$ Rotations	0.2917	0.0747	0.1232	0.0627
T3: $2\times 180^{\circ}$ Rotations + Starfish Landmark	0.3514	0.0812	0.0867	0.0905
T4: $2\times 180^{\circ}$ Rotations + Depth Change	0.1954	0.0489	0.0639	0.0134
T5: $2\times 180^{\circ}$ Rotations + Depth Change + Starfish Landmark	0.1651	0.0673	0.0412	0.0097

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
$\hookrightarrow$ 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
$\hookrightarrow$ No landmark – Starfish revisit distance (m)	0.58m
$\hookrightarrow$ 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.

Robot Localization

Robot localization estimates a robot’s position and orientation from onboard sensors, maps, and observations of its surroundings.

See all 177 papers

DIVO: Continuous-time DVL-Inertial-Visual Odometry for Unmanned Underwater Vehicles
Jul 6, 2026Kyungmin Jung, Angad Bajwa, Junha Yoo +2Underwater Robotics Visual-Inertial Odometry
Department of Mechanical Engineering, McGill University, Montreal, QC, Canada
SURGE: Sonar-fUsed Reconstruction and localization via image-gated Graph Estimation
Oct 5, 2026Mohammed Ibrahim M, Vallabh Deogaonkar, Trung Dong +3Underwater 3D Reconstruction Robot Localization
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
APVI-SLAM: Real-Time Acoustic-Pressure-Visual-Inertial Localization and Photorealistic Mapping System in Complex Underwater Environment
Jul 7, 2026Hanwen Zhang, Yipeng Zhu, Xiaopeng Guo +2Underwater Robotics Visual-Inertial SLAM
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

Jul 6, 2026J/K move · Enter open · S save

DIVO: Continuous-time DVL-Inertial-Visual Odometry for Unmanned Underwater Vehicles

Kyungmin Jung, Angad Bajwa, Junha Yoo, Arturo Del Castillo Bernal+1

Department of Mechanical Engineering, McGill University, Montreal, QC, Canada

Underwater Robotics Visual-Inertial Odometry Autonomous Underwater Navigation Robot State Estimation Multimodal Sensor Fusion

Open paper AskPDF arXiv

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.

Abstract

Robot Localization

Explore similar work

DIVO: Continuous-time DVL-Inertial-Visual Odometry for Unmanned Underwater Vehicles

SURGE: Sonar-fUsed Reconstruction and localization via image-gated Graph Estimation

APVI-SLAM: Real-Time Acoustic-Pressure-Visual-Inertial Localization and Photorealistic Mapping System in Complex Underwater Environment