Contact-Aided Factor-Graph Localization for Underwater Sampling

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

Source-linked AI summary

Contact-Aided Factor-Graph Localization for Underwater Sampling

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

arXiv:2608.26932v1Aug 27, 2026cs.RO

TL;DR

Perception-degraded underwater sampling challenges localization because planar seabeds and weak visual tracking leave inertial–DVL fusion without structural drift correction. The paper introduces a contact-aided factor-graph framework that fuses suction contact with adaptive visual odometry, learned detections, and onboard sensors. Experiments show reduced drift and improved object revisit accuracy compared with filtering-based navigation and contact-free graph formulations.

Problem
Planar, texture-sparse seabeds cause scale ambiguity, lateral degeneracy, and inconsistent tracking, while inertial–DVL fusion alone lacks structural drift correction.
Method
The framework tightly couples suction contact, adaptive visual odometry, learned object detections, and onboard sensing in a smoothing-based factor graph with online initialization.
Results
Contact-induced constraints significantly reduce trajectory drift and improve object revisit accuracy across simulation, tank, and harbor evaluations.
Takeaways & Limitations
Physical interaction functions as a localization primitive that supplies informative geometric constraints in perception-degraded underwater environments.

Abstract

from arXiv · show

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

I. INTRODUCTION

Close-range underwater sampling suffers from drift and weak visual geometry, so the framework uses physical suction contact as a high-confidence localization constraint within a factor graph.

Motivation: Low-altitude, down-looking AUVs face planar, texture-sparse scenes that weaken monocular visual estimation and conventional navigation.The setting lacks GPS, while inertial–DVL dead reckoning accumulates drift and underwater imagery is degraded.
Motivation: Suction contact creates a metrically grounded, viewpoint-invariant constraint that can anchor the vehicle without appearance-based place recognition.The contact point is modeled as a fixed landmark within the smoothing-based SLAM formulation.
Contributions: Adaptive uncertainty scaling addresses planar visual degeneracy, while high-confidence suction factors provide implicit loop closures and hybrid refinement supports intermittent visual structure.The system alternates between landmark-augmented and pose-only refinement for real-time robustness.
Contributions: The framework tightly couples contact, visual, inertial, DVL, and learned detection information in an incrementally initialized factor-graph backend.It initializes fully online and is evaluated in simulation, tanks, and harbor deployments.
Results: Contact-induced constraints significantly reduce trajectory drift and improve object revisit accuracy relative to filtering-based navigation and contact-free graph formulations.The paper presents physical interaction as a localization primitive for perception-degraded underwater environments.

II. RELATED WORK

The framework combines conventional underwater sensors and visual information in a contact-aided smoothing formulation that initializes online and maintains a fused inertial backbone when visual structure is weak.

Related Work: Underwater navigation lacks GPS and remains vulnerable to drift when reliable loop closures or exteroceptive structure are unavailable.Texture-sparse benthic environments limit the effectiveness of conventional visual and multimodal approaches.
Framework: Vehicle poses, velocities, IMU biases, and landmarks are jointly estimated by fusing navigation measurements, visual observations, object detections, and contact constraints.The estimator is implemented as a contact-aided smoothing problem on a factor graph.
Framework: The system initializes fully online from IMU, magnetometer, pressure, and DVL data without requiring stationarity or a dedicated bootstrapping phase.Initial pose, velocity, and Gaussian priors are constructed as sensor data become available.
Inertial Sensors: IMU preintegration links consecutive states while DVL, pressure, and magnetometer factors constrain motion, velocity, depth, and heading.These factors form the fused inertial backbone used when visual structure is weak or intermittent.
Inertial Sensors: Together, the inertial factors provide tightly coupled constraints on inter-node motion, horizontal velocity, vertical position, and heading.This supports stable pose estimation despite weak or intermittent visual structure.

B. Down-Looking Visual Odometry

The down-looking visual-odometry pipeline recovers metric scale from auxiliary sensors and adaptively downweights visually degenerate observations before adding them to the factor graph.

Down-Looking Visual Odometry: Monocular VO is noisy and scale-ambiguous in planar, low-texture, low-altitude operation, motivating multi-sensor scale recovery and adaptive uncertainty modeling.The pipeline also applies contrast enhancement, ORB features, BRISK fallback, and ratio-test matching.
Scale Recovery: Metric scale is recovered from DVL or sonar altitude, pressure-derived depth, or IMU fallback before forming the relative camera pose.Fixed camera–body extrinsics transform the scaled motion into the body frame.
Factor Integration: The scaled body-frame motion is incorporated as a BetweenFactor, alongside a velocity constraint corrected for the camera–IMU lever arm.These factors integrate visual motion with the sensor geometry.
Uncertainty Modeling: Visual covariance is adaptively scaled using the inlier ratio, smoothed by an exponential moving average, and clamped within bounded limits.This preserves informative updates while reducing the influence of unreliable observations.

C. Visual Landmark Detections

The landmark-detection pipeline converts learned object detections into uncertainty-aware bearing–range factors and expresses landmarks in the end-effector frame for contact-consistent mapping.

Detection: A YOLOv8 detector provides object class, pixel center, confidence, and bounding-box information from the down-looking camera stream.The detector is trained on a public starfish dataset.
Range and Bearing: Slant range is estimated from bounding-box height using a pinhole model and assumed object size, with DVL altitude bounding the estimate when available.The range relation uses focal length, object size, and image height.
Range and Bearing: Pixel centers are back-projected into normalized camera-frame bearings and transformed into the world frame using fixed camera–body extrinsics and the current vehicle pose.Persistent landmarks receive incremental detection updates.
Factor Modeling: Each detection contributes a bearing–range factor whose uncertainty is inflated for low-confidence or small bounding-box detections.This links neural detections to vehicle pose while limiting the influence of less reliable observations.
Contact Consistency: End-effector-frame landmark expressions directly anchor visual detections to contact events, supporting contact-consistent mapping and implicit loop closure.The representation connects learned detections with physical interaction constraints.

D. Hybrid Local Bundle Adjustment

The framework uses a sliding-window local bundle adjustment that switches between landmark-augmented and pose-only refinement according to available visual structure. This preserves robustness in sparse scenes while reducing short-term drift within an incremental iSAM2 backend.

D. Hybrid Local Bundle Adjustment: The sliding window optimizes the most recent poses and uses landmark-augmented refinement when connected visual landmarks are available.Otherwise, it retains only pose-related variables and factors.
D. Hybrid Local Bundle Adjustment: Poses outside the local window are anchored with strong priors to prevent gauge freedom during nonlinear least-squares optimization.
D. Hybrid Local Bundle Adjustment: Adaptive switching preserves robustness in visually sparse conditions while exploiting landmark constraints when available.
D. Hybrid Local Bundle Adjustment: Sliding-window refinement reduces short-term drift and improves local smoothness without compromising incremental iSAM2 performance.

E. Manipulator-Based Contact Sensing

The manipulator supplies contact-based geometric constraints through its suction end-effector, whose contact events are incorporated into the factor graph as point and bearing–range measurements. Recontact with mapped objects creates implicit loop closures without explicit place recognition.

E. Manipulator-Based Contact Sensing: Joint-encoder forward kinematics provides the end-effector pose, while acceleration-level inverse kinematics supports precise positioning during suction interactions.
E. Manipulator-Based Contact Sensing: A suction engagement event becomes a point measurement, and nearby mapped landmarks receive a bearing–range factor linking the current pose to the landmark.The nearby-landmark condition is ||l_j − p_ee|| < ϵ.
E. Manipulator-Based Contact Sensing: When no nearby landmark exists, the system initializes a landmark at the contact point and adds a zero-range contact factor.A tight isotropic noise model enforces strong geometric consistency in both cases.
E. Manipulator-Based Contact Sensing: Recontact with a previously mapped object induces an implicit loop closure without requiring appearance-based place recognition.The manipulator therefore provides sparse, high-confidence constraints during prolonged low-visibility operation.

IV. EXPERIMENTAL SETUP

The evaluation combines physics-based simulation with controlled tank experiments and harbor deployments designed to stress observability in planar, low-texture underwater conditions. Trials include structured motion, depth variation, rotations, and repeated approaches to a benthic target.

IV. EXPERIMENTAL SETUP: Simulation uses Stonefish with perfect ground-truth odometry for quantitative validation.
IV. EXPERIMENTAL SETUP: The first-tank study contains five trajectories spanning straight motion, repeated rotations, starfish landmarks, depth variation, and their combinations.The figure marks the starfish in red and highlights improved-estimation segments with dashed green lines.
IV. EXPERIMENTAL SETUP: Real-world trials cover two water tanks and one harbor site under controlled and naturally varying illumination and turbidity.The tanks measure 100 m × 7.8 m × 4.35 m and 40 m × 4 m × 2 m; harbor trials cover approximately 10 m × 10 m × 5 m.
IV. EXPERIMENTAL SETUP: Large-tank trajectories include straight motion, two 180° yaw turns, combined yaw rotations with depth variation, and repeated approaches to a starfish target.

A. Visual Degeneracy Characterization

Underwater sequences exhibit predominantly planar, low-texture structure with weak temporal feature retention and limited or inconsistent optical flow. These conditions destabilize depth estimation and cause classical monocular reconstruction and tracking systems to fail.

A. Visual Degeneracy Characterization: Harbor frames average 418 detected keypoints with 42% retained between frames, while the first tank averages 781.8 detections with 47% temporal retention.
A. Visual Degeneracy Characterization: Gradient magnitudes range from 9.38–15.60, indicating limited photometric structure across the evaluated sequences.
A. Visual Degeneracy Characterization: Optical flow is either insufficient for meaningful parallax at approximately 1.24 px or inconsistent at up to 31.22 px, producing unstable depth estimation.
A. Visual Degeneracy Characterization: COLMAP fails to initialize reliable sparse reconstructions, while ORB-SLAM3, VINS-MONO, and ROVIO repeatedly lose tracking or diverge.

V. RESULTS

The evaluation spans controlled tank, ablation, and Stonefish simulation studies designed to assess observability, sensing contributions, and trajectory accuracy.

The experiments assess estimator behavior under planar degeneracy, sensing and landmark contributions, and task-level revisit consistency.
Under straight motion, ATE is 0.0990 m with yaw RMSE 0.0524 rad, while two 180° rotations increase them to 0.2917 m and 0.1232 rad.
Depth variation improves conditioning, reducing ATE to 0.1954 m and yaw RMSE to 0.0639 rad; combined depth and landmark constraints achieve ATE 0.1651 m and yaw RMSE 0.0412 rad.
The ablation study finds that removing visual refinement or weakening sensor coupling degrades performance, supporting tightly coupled fusion with local optimization.

B. Stonefish Simulation: Ground-Truth Validation

Stonefish simulation compares estimated trajectories with perfect-odometry ground truth across motion patterns, including rotations, depth–yaw excitation, and starfish dives.

For well-conditioned forward–lateral and depth–yaw motions, ATE remains below 0.1 m, while multiple rotations produce ATE 0.1635 m with small relative pose errors.
Continuous planar rotations produce the largest drift, with ATE 0.998 m, despite low RPE of 0.0032 m.
The semi-8 trajectory includes two starfish dives and evaluates landmark-plus-pump constraints for metrically consistent interaction-site anchoring.

C. Second Tank: Landmark and Contact Evaluation

Second-tank and harbor experiments evaluate repeated benthic interactions, showing low short-horizon errors and improved revisit consistency when visual and contact constraints are added.

Repeated interaction trials have global ATE from 0.4464 m to 1.0158 m, while relative pose errors remain low at 0.0035 m–0.0152 m.
Adding landmarks reduces revisit error from 0.58 m without landmarks, while the supplied passage truncates the subsequent contact result.
In harbor conditions, revisit error decreases from 1.23 m without landmarks to 1.12 m with landmarks and 0.90 m with pump events.
The full evaluation reports that contact constraints consistently improve revisit accuracy and spatial consistency at manipulation sites across simulation, tanks, and harbor deployment.