PaperlayerView exact revision on arXiv

Source-linked AI summary

SVIn2: An Underwater SLAM System using Sonar, Visual, Inertial, and Depth Sensor

Sharmin Rahman, Alberto Quattrini Li, Ioannis Rekleitis

arXiv:1810.03200v3cs.RO

TL;DR

Reliable underwater SLAM is challenging because visual degradation and accumulated drift can undermine localization. SVIn2 combines sonar, visual, inertial, and depth sensing with robust initialization, image preprocessing, loop closure, and relocalization; underwater evaluations report maintained scale without divergence where other systems lose tracking or scale.

  • Problem

    Reliable SLAM remains an open problem for autonomous underwater mapping, where degraded visual features can produce inaccurate estimates or tracking loss and trajectories can accumulate drift.

    Source: §I · p. 1

  • Method

    SVIn2 extends sonar-visual-inertial estimation with depth measurements, robust initialization, underwater image preprocessing, loop closure, and relocalization.

    Source: §I · p. 1

  • Results

    Underwater evaluations found other state-of-the-art systems lost tracking and/or failed to maintain consistent scale, while SVIn2 maintained correct scale without diverging.

    Source: §I · p. 2

  • Takeaways & Limitations

    SVIn2 is targeted to underwater environments and can also operate out of water by disabling inapplicable sensors.

    Source: §V · p. 6

    Online adaptation for the limited computational resources of the Aqua2 AUV remains under consideration.

    Source: §V · p. 7

Abstract

from arXiv · show

This paper presents a novel tightly-coupled keyframe-based Simultaneous Localization and Mapping (SLAM) system with loop-closing and relocalization capabilities targeted for the underwater domain. Our previous work, SVIn, augmented the state-of-the-art visual-inertial state estimation package OKVIS to accommodate acoustic data from sonar in a non-linear optimization-based framework. This paper addresses drift and loss of localization -- one of the main problems affecting other packages in underwater domain -- by providing the following main contributions: a robust initialization method to refine scale using depth measurements, a fast preprocessing step to enhance the image quality, and a real-time loop-closing and relocalization method using bag of words. An additional contribution is the introduction of depth measurements from a pressure sensor to the tightly-coupled optimization formulation. Experimental results on datasets collected with a custom-made underwater sensor suite and an autonomous underwater vehicle from challenging underwater environments with poor visibility demonstrate performance never achieved before in terms of accuracy and robustness.

I. INTRODUCTION

Reliable SLAM remains an open problem for autonomous underwater mapping, where visual degradation and accumulated drift challenge localization. SVIn2 extends SVIn with depth, improved initialization, image preprocessing, loop closure, and relocalization; underwater evaluations report maintained scale without divergence where other systems lose tracking or scale.

I. INTRODUCTION

  • Autonomous underwater vehicles could automate mapping now often done by divers using manual measurements or handheld sensors, but reliable SLAM remains an open problem.Underwater exploration and mapping are relevant to the economy, conservation, and scientific discovery.Source: §I · p. 1
  • Acoustic navigation data collection can be expensive and sometimes unsuitable in highly unstructured underwater environments.The cited acoustic sensors include DVL, USBL, and sonar [3],,,,.Source: §I · p. 1
  • Underwater vision is degraded by suspended particulates, blurriness, and light and color attenuation, producing poorly defined features and outliers that can cause inaccurate estimates or tracking loss.A prior study found vision-based state-estimation packages were not straightforward to deploy underwater.Source: §I · p. 1
  • SVIn2 targets underwater environments and adapts to sensor configurations combining acoustic, visual, inertial, and depth data.The system is intended for different sensor suites and underwater vehicles.Source: §I · p. 1
  • SVIn2 extends SVIn with depth measurements, more robust initialization, image enhancement, loop closure, and relocalization to improve trajectory estimation.SVIn fused acoustic, visual, and inertial data, but drift could accumulate on long trajectories.Source: §I · pp. 1–2
  • Underwater evaluations found other state-of-the-art systems lost tracking and/or failed to maintain consistent scale, while SVIn2 maintained correct scale without diverging.The evaluation used data from a custom sensor suite and an Aqua2 vehicle.Source: §I · p. 2

II. RELATED WORK

Related work spans acoustic underwater navigation and visual or visual-inertial state estimation. The paper builds on OKVIS and prior sonar fusion, while addressing tracking, loop-closure, and initialization considerations relevant to underwater deployment.

II. RELATED WORK

  • Sonar-based systems use acoustic sensing for underwater SLAM, while vision and visual-inertial systems have also been developed for underwater reconstruction and navigation.Examples include Sunfish with multibeam sonar, FOG IMU, acoustic DVL, and pressure-depth sensing, and comparisons of acoustic and visual localization.Source: §II · p. 2
  • Feature-based visual-inertial systems such as OKVIS and Visual-Inertial ORB-SLAM optimize IMU and reprojection errors, while VINS-Mono uses KLT optical flow in a local window.KLT features support real-time operation on low-cost embedded systems, but can result in tracking failure in challenging underwater environments.Source: §II · p. 2
  • Loop closure recognizes previously seen places to mitigate state-estimate drift; ORB-SLAM, its IMU extension, and VINS-Mono use bag-of-words approaches.FAB-MAP, is another appearance-based place-recognition method.Source: §II · p. 2
  • Visual-inertial systems require initialization; VINS-Mono aligns monocular vision with inertial measurements, while ORB-SLAM with IMU estimates biases after monocular SLAM observes pose.Source: §II · p. 2
  • SVIn2 extends OKVIS, building on sonar-fused visual-inertial odometry shown to improve trajectory estimates under varying visibility, while adding depth, loop closure, and more robust initialization.Source: §II · p. 2

III. PROPOSED METHOD

SVIn2 is a modular state-estimation system that can use stereo imagery, IMU, sonar, and depth sensing. Its method adds underwater image preprocessing and proceeds through initialization, sensor-fusion optimization, loop closure, and relocalization.

III. PROPOSED METHOD

  • The full system uses stereo camera, IMU, sonar, and depth sensing, with sonar and depth optionally disabled for visual-inertial operation.Source: §III · p. 2
  • Contrast adjustment and histogram equalization are added to improve underwater feature detection, using a CLAHE filter.The preprocessing addresses low visibility, dynamic obstacles, light and color attenuation, and poor contrast.Source: §III · pp. 2–3
  • The method describes initialization, sensor-fusion optimization, loop closure, and relocalization after defining the state.Source: §III · p. 3

A. Notations and States

The system represents sensor frames and transformations explicitly, then estimates an IMU-referenced robot state and its error in minimal coordinates.

A. Notations and States

  • The sensor suite defines camera, IMU, sonar, depth, and world coordinate frames as C, I, S, D, and W, respectively.Source: A. Notations and States · p. 3
  • A transformation between frames X and Y is represented by a homogeneous matrix containing rotation and position, with rotation also represented by a quaternion.Source: A. Notations and States · p. 3
  • The estimated robot state contains IMU-referenced position, attitude, velocity, and gyroscope and accelerometer biases; its error state uses minimal coordinates.The position, attitude, and velocity are expressed with respect to the world frame.Source: A. Notations and States · p. 3

B. Tightly-coupled Non-Linear Optimization with Sonar-Visual-Inertial-Depth measurements

SVIn2 combines visual, inertial, sonar, and depth errors in a tightly coupled nonlinear optimization. Sonar constrains visual features near detected obstacles, while depth measurements constrain position along the world z direction.

B. Tightly-coupled Non-Linear Optimization with Sonar-Visual-Inertial-Depth measurements

  • The optimization combines stereo reprojection, IMU pre-integration, sonar-range, and depth error terms to estimate the robot state.All error terms are added in the Ceres Solver nonlinear optimization framework.Source: B. Tightly-coupled Non-Linear… · pp. 3–4
  • The reprojection term measures the discrepancy between a camera keypoint and its stereo-projected landmark, while the IMU term represents pose, speed, and bias error between successive camera measurements.IMU measurements are pre-integrated between successive camera measurements.Source: B. Tightly-coupled Non-Linear… · p. 3
  • Sonar range error compares sonar-point distance with the distance to a nearby visual patch, assuming the patch is small and approximately coplanar with the sonar point.The resulting constraint is used to correct the robot position.Source: B. Tightly-coupled Non-Linear… · p. 3
  • Depth error is the difference between the robot’s world-z position and the pressure-sensor depth measurement, used to correct its position.Depth is extracted along gravity, aligned with the world z axis through tightly coupled IMU integration.Source: B. Tightly-coupled Non-Linear… · p. 4

C. Initialization: Two-step Scale Refinement

SVIn2 initializes with stereo, IMU, and depth information to refine scale, velocity, and gravity. The two-step scale refinement uses depth first and then aligns the refined stereo measurement with IMU pre-integration.

C. Initialization: Two-step Scale Refinement

  • Underwater initialization uses stereo, IMU, and depth constraints on scale because occlusion and scarce trackable features make robust initialization especially important.The method does not use sonar at initialization because its 360° scan and the camera’s forward view create a temporal mismatch between sonar ranges and visual features when the robot is stationary.Source: C. Initialization: Two-step Scale… · p. 4, Figure 3 · p. 4
  • Initialization waits for a minimum number of trackable visual features; 15 worked well in the experiments.The initial stereo scale is then refined in two steps.Source: C. Initialization: Two-step Scale… · p. 4
  • Depth measurements provide the first refinement of the initial stereo scale factor.The depth-based transformation accounts for the initial displacement along the z axis from the IMU.Source: C. Initialization: Two-step Scale… · p. 4
  • The second refinement aligns the depth-refined stereo measurement with IMU pre-integration; initialization also approximates initial velocity and gravity.The IMU pre-integration terms describe motion between consecutive keyframes and are obtained from IMU measurements.Source: C. Initialization: Two-step Scale… · p. 4

D. Loop-closing and Relocalization

SVIn2 uses a bag-of-words database and pose graph to detect loops outside the local marginalization window, geometrically validate matches, and correct drift through global relocalization and pose-graph optimization.

D. Loop-closing and Relocalization

  • Loop detection uses only locally tracked keypoint descriptors in a DBoW2 database, searching beyond the marginalization window to address accumulated pose drift.No new features are detected during loop closure.Source: D. Loop-closing and Relocalization · p. 5
  • A loop candidate must score at least as highly as neighboring pose-graph keyframes; the highest-scoring candidate is geometrically checked using 2D–2D and 3D–2D matches with PnP RANSAC outlier rejection.Pose-graph edges are formed when the matched-keypoint ratio exceeds 0.75.Source: D. Loop-closing and Relocalization · p. 5
  • On loop detection, global relocalization sends the pose drift back to the local optimization thread, which adds an optimization step using matched landmarks for sonar and reprojection errors.The local window pose is aligned with the loop keyframe pose in the pose graph.Source: D. Loop-closing and Relocalization · p. 5
  • A 6-DoF pose-graph optimization corrects drift using relative constraints between poses, with a Huber loss to potentially downweight incorrect loops.The objective uses estimates from local sonar-visual-inertial-depth optimization.Source: D. Loop-closing and Relocalization · p. 5

IV. EXPERIMENTAL RESULTS

The evaluation includes a standard dataset for quantitative validation of loop closure and initialization, alongside qualitative tests on underwater datasets collected with a custom sensor suite and an Aqua2 AUV.

IV. EXPERIMENTAL RESULTS

  • The standard-dataset evaluation quantitatively tests whether loop closure and initialization work above water.SVIn2 is compared with VINS-Mono, basic OKVIS, and the GRASP lab’s MSCKF implementation.Source: §IV · p. 5
  • Underwater tests qualitatively evaluate SVIn2 on datasets collected with a custom-made sensor suite and an Aqua2 AUV.Source: §IV · p. 5
  • The evaluation uses a standard dataset for the quantitative comparison and several underwater datasets for qualitative testing.Source: §IV · p. 5

A. Validation on Standard dataset

The EuRoC evaluation disables sonar and depth to assess loop closure, comparing SVIn2 with established visual-inertial systems. SVIn2 reduces translation RMSE relative to OKVIS on every Machine Hall sequence.

A. Validation on Standard dataset

B. Underwater datasets

Across four challenging underwater datasets, SVIn2 tracks and closes loops where comparison systems lose track or fail to correct drift; evaluation is qualitative because ground truth is unavailable.

B. Underwater datasets

  • The four runs cover a hazy bus, two dark cavern datasets, and an AUV cemetery survey; the sensor suite records stereo, inertial, depth, and acoustic data.The Aqua2 AUV collected the cemetery data; underwater evaluation is qualitative because GPS-denied, unstructured settings lack ground truth.Source: B. Underwater datasets · p. 6, Figure 5 · p. 6
  • For trajectory comparisons, SVIn2’s sonar and depth fusion was disabled, and each package’s original scale was retained.Source: B. Underwater datasets · p. 6
  • In the submerged-bus run, SVIn2 tracked and corrected the loop, while VINS-Mono lost tracking at high exposure and OKVIS needed image preprocessing to keep tracking.Source: B. Underwater datasets · p. 6
  • In Cavern1, SVIn2 detected and closed loops; VINS-Mono tracked throughout but had empirically incorrect scale, while OKVIS produced a good trajectory.Source: B. Underwater datasets · p. 6
  • In Cavern2, SVIn2 tracked, reduced drift, and closed loops, whereas OKVIS could not rejoin the expected prior pose as drift accumulated.Source: B. Underwater datasets · p. 6
  • At the cemetery, SVIn2 fused and corrected loops while both VINS-Mono and OKVIS tracked; VINS-Mono did not reduce trajectory drift.Source: B. Underwater datasets · p. 6

V. CONCLUSIONS

SVIn2 combines robust initialization, depth/sonar/visual/inertial sensor fusion, and loop closure, with reported performance on a standard benchmark and diverse underwater datasets. It is aimed at underwater use but can also operate out of water with inapplicable sensors disabled.

V. CONCLUSIONS

  • Experiments on a standard benchmark and different underwater datasets demonstrate excellent performance for SVIn2.Source: §V · p. 6
  • SVIn2 combines robust initialization, depth, sonar, visual, and inertial data fusion, and loop-closure capabilities.Source: §V · p. 6
  • The system is specifically targeted for underwater environments but can also work out of water when inapplicable sensors are disabled.Source: §V · p. 6
  • The evaluation includes 53 m, 87 m, and 155 m trajectories in submerged bus and cave settings, plus an 80 m cemetery trajectory without sonar data.Source: Figure 6 · p. 7, Figure 7 · p. 7, Figure 8 · p. 7
  • Online adaptation for Aqua2’s limited computational resources is under consideration.Source: §V · p. 7
  • Maintaining trajectory attitude and estimating distance traveled would greatly enhance the vehicle’s autonomous capabilities.Source: §V · p. 7
  • Accurately modeling surrounding structures would enable Aqua2 and other vision-based underwater vehicles to operate near and through caves, shipwrecks, and canyons.Source: §V · p. 8
Loading 1810.03200v3…