SVIn2: Robust Underwater SLAM System

• SVIn2 is a tightly-coupled underwater SLAM system that integrates stereo, inertial, pressure-based depth, and optional sonar sensing to maintain robust localization under poor visibility.
• It uses a sliding-window non-linear optimization backend with online loop closure and relocalization to correct drift and ensure metric consistency over long trajectories.
• SVIn2 serves both as a real-time front end for dense 3D mapping and as a generator of sparse trajectories for post-dive reconstruction in challenging underwater environments.

Searching arXiv for SVIn2 and related underwater SLAM papers. SVIn2 is a tightly-coupled, keyframe-based underwater SLAM system that fuses stereo vision, inertial sensing, pressure-based depth, and optionally mechanical-scanning sonar within a single non-linear optimization backend with online loop-closing and relocalization (Rahman et al., 2018). It was introduced for underwater environments characterized by poor visibility, haze, non-uniform illumination, low contrast, and drift-sensitive long trajectories, and it has subsequently been used both as a real-time visual-inertial front end for dense 3D mapping and as a trajectory-and-sparse-map generator for cave-scale post-dive reconstruction workflows (Rahman et al., 2018, Wang et al., 2023, Chatzispyrou et al., 8 Jul 2025). In the later literature, the name denotes both the original four-modality system and a closely related visual-inertial formulation augmented by depth and, when available, sonar factors.

1. Origin and problem setting

SVIn2 was presented in "SVIn2: An Underwater SLAM System using Sonar, Visual, Inertial, and Depth Sensor" as an extension of the earlier SVIn system, which had augmented OKVIS to accommodate acoustic data from sonar in a non-linear optimization-based framework (Rahman et al., 2018). The stated motivation was drift and loss of localization in the underwater domain, especially under poor visibility and weak visual texture, with the system adding a robust initialization method to refine scale using depth measurements, a fast preprocessing step to enhance image quality, a real-time loop-closing and relocalization method using bag of words, and tightly-coupled pressure-sensor depth measurements (Rahman et al., 2018).

The original formulation targets underwater localization and mapping on platforms such as a custom-made underwater sensor suite and an autonomous underwater vehicle, with all sensor streams wired into the vehicle’s on-board computer and synchronized to a consistent timebase (Rahman et al., 2018). Later work reused SVIn2 in two distinct ways. In "Real-Time Dense 3D Mapping of Underwater Environments," SVIn2 serves as a robust VIO method providing robust, scale-correct camera poses in real time for a CPU-only stereo-matching and depth-fusion pipeline (Wang et al., 2023). In "Mapping the Catacombs," SVIn2 is used to estimate the trajectories of multiple action cameras together with a sparse point cloud in an underwater cave, after which its keyframes and estimated camera poses are used as input to COLMAP for dense reconstruction of selected areas (Chatzispyrou et al., 8 Jul 2025).

A plausible implication is that SVIn2 functions less as a single immutable implementation than as a core underwater VI-SLAM design pattern: tightly-coupled optimization over keyframes, explicit metric observability through depth sensing, and drift control through loop closure, with sonar fusion remaining optional in later deployments.

2. Sensor suite, synchronization, and state representation

In its original form, SVIn2 fuses four sensing modalities: a stereo camera with two monochrome imagers at 15 Hz, a MEMS IMU at 100 Hz, a mechanical-scanning sonar at 100 Hz with 360° sweep, and a pressure-based depth sensor at 1 Hz (Rahman et al., 2018). Cameras and IMU share a hardware trigger, while sonar and depth readings are stamped by the same system clock and interpolated to the closest visual-keyframe timestamp so that every residual in the optimization uses a consistent timebase (Rahman et al., 2018).

The later dense-mapping pipeline uses a calibrated stereo camera pair at 15 Hz and 800×600 px, a tactical-grade IMU at 100 Hz, and optional depth-pressure sensor and imaging sonar (Wang et al., 2023). The cave-mapping deployment instead uses three GoPro Hero9 rigs at 30 Hz, GoPro’s internal MEMS IMU at 100 Hz, and a Shearwater Perdix2 dive computer providing absolute depth at 0.1 Hz (Chatzispyrou et al., 8 Jul 2025). In that setting, a fixed transform $T_{cb}=(R_{cb},p_{cb})$ from body to each camera is estimated offline via a small-motion sequence in front of an AprilGrid (Chatzispyrou et al., 8 Jul 2025).

The per-keyframe state in SVIn2 is consistently built around pose, velocity, and inertial biases. In the 2018 formulation, the keyframe state is

$x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$

with position and orientation of the IMU, world-frame velocity, and gyro and accelerometer biases (Rahman et al., 2018). In the later notation used for dense mapping, the state at keyframe $i$ is

$x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$

to which 3D structure variables $X_l$ are appended for landmarks triangulated over multiple keyframes (Wang et al., 2023). In the cave-mapping paper, the state is written as

$$x_i = [R_{wb_i}, p_{wb_i}, v_{wb_i}, b_{g_i}, b_{a_i}],$$

again with rotation, position, velocity, and slowly varying gyroscope and accelerometer biases (Chatzispyrou et al., 8 Jul 2025).

These formulations are mathematically equivalent at the architectural level: each represents a sliding-window factor-graph or bundle-adjustment problem over keyframes, inertial motion, visual observations, and additional underwater-specific sensing constraints.

3. Initialization, observability, and underwater-specific fusion

A central feature of SVIn2 is its treatment of scale and vertical observability. The 2018 paper describes a two-step initialization and scale-refinement procedure using the pressure sensor (Rahman et al., 2018). First, the camera’s z-position from pure stereo, $_W p_C^k$, is related to the absolute depth measurement $_W p_D^k$ by

$_W p_D^k \;=\; s_1\;_W p_C^k\;+\;_{W}R_{C}\,p_{CD},$

yielding a first scale refinement

$_W p_{r1,C}^k = s_1\;_W p_C^k.$

Second, the depth-refined stereo trajectory is aligned to IMU preintegrations using the standard on-manifold preintegrated increments and a linear least-squares problem in the stacked unknowns

$x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$0

thereby estimating $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$1, gravity, and initial velocities before full metric alignment of stereo to IMU (Rahman et al., 2018).

The later cave-mapping work expresses the same principle in observability terms. It states that in a pure VIO system the scale and the vertical drift are weakly constrained, while adding dive-computer depth $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$2 as the unary factor

$x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$3

tightly binds $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$4 to true depth, thus making the vertical axis and scale fully observable (Chatzispyrou et al., 8 Jul 2025). The same work also notes that SVIn2 leverages the strong coupling of gravity in the IMU propagation to recover roll and pitch, without adding an explicit roll/pitch sensor (Chatzispyrou et al., 8 Jul 2025).

This use of pressure-derived depth is one of the defining distinctions of SVIn2 relative to generic VIO. In underwater deployments, absolute depth is both physically available and highly informative; SVIn2 incorporates it not merely as an auxiliary measurement but as a structural constraint on metric consistency and long-horizon drift.

4. Non-linear optimization and residual structure

SVIn2’s core backend is a sliding-window non-linear least-squares optimizer. The 2018 system formulates a combined cost over visual reprojection, IMU preintegration, sonar range, and depth residuals: $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$5 where $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$6 is the visual reprojection residual, $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$7 the IMU preintegration residual, $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$8 the sonar range residual, and $x_R^k = \bigl[\,_W p_I^k,\;_W q_I^k,\;_W v_I^k,\;b_g^k,\;b_a^k\bigr]^T,$9 the depth residual (Rahman et al., 2018). The problem is solved in real time by Ceres-Solver, with IMU biases, visual-landmark positions, sonar-point associations, and the full pose-graph inside the window jointly optimized (Rahman et al., 2018).

The dense-mapping paper presents the same backend from the VIO perspective. IMU propagation uses classic IMU-Euler-Newton kinematics and preintegrated increments

$i$0

$i$1

$i$2

with residual

$i$3

weighted by the preintegration covariance (Wang et al., 2023). Visual coupling is provided by reprojection residuals

$i$4

weighted by the inverse image-covariance (Wang et al., 2023).

The cave-mapping work generalizes the objective to a factor graph over keyframes, IMU measurements, visual observations, and depth: $i$5 with all Jacobians $i$6 derived in closed form (Chatzispyrou et al., 8 Jul 2025).

Across these variants, the common principle is unchanged: SVIn2 estimates a metric trajectory by jointly minimizing residuals from complementary sensors rather than cascading independently optimized modules.

5. Front-end processing, loop closure, and relocalization

Underwater imagery is subject to haze, non-uniform illumination, and severe color and contrast attenuation. The original system therefore applies Contrast-Limited Adaptive Histogram Equalization (CLAHE) before FAST keypoint detection and ORB-descriptor extraction (Rahman et al., 2018). The cave-mapping deployment instead describes an image front end with adaptive thresholding and RANSAC-backed feature selection to cope with low-contrast, hazy images, together with tightened residual gating for reprojection to suppress mismatches on noisy walls (Chatzispyrou et al., 8 Jul 2025). The dense-mapping pipeline identifies feature extraction using ORB, stereo and temporal matching, and a sliding-window nonlinear optimization backend with sparsely marginalizing out old keyframes (Wang et al., 2023).

Loop-closing and relocalization are also central. In the original paper, SVIn2 embeds a DBoW2 place-recognition module. For each incoming keyframe, ORB descriptors of tracked keypoints are inserted into a BoW database; a sparse pose graph $i$7 is maintained; loop candidates outside the current marginalization window are queried; geometric verification is performed by 2D–2D descriptor matching and 3D–2D PnP+RANSAC; and accepted loops trigger both local pose correction and global 6-DoF pose-graph optimization with a robust Huber kernel on loop edges (Rahman et al., 2018). The dense-mapping paper summarizes this as DBoW2 vocabulary plus robust PnP, with a two-stage loop-closing scheme consisting of fast DBoW2 candidate detection followed by robust geometric verification using 3D–2D PnP with outlier rejection (Wang et al., 2023). The cave-mapping paper states that if a loop candidate is found, a pose-graph optimization is triggered to reduce long-term drift, and that during relocalization the frontend attempts to match new frames to existing keyframes, with success resetting the state to the last optimized keyframe (Chatzispyrou et al., 8 Jul 2025).

These modules address one of the explicit failure modes identified in underwater SLAM: open-loop drift or complete localization failure in visually repetitive, low-texture, or long-duration trajectories.

6. Optional sonar fusion and integration into mapping pipelines

Sonar is optional in later SVIn2-based systems but fundamental to the original 2018 formulation. There, raw sonar range returns are pre-filtered with a small median filter in the angular domain, converted into 3D points in the world frame via known sonar-to-IMU extrinsics, and associated during backend optimization with a small visual-patch centroid found nearby in the same keyframe (Rahman et al., 2018). The sonar residual is written as

$i$8

where $i$9 is the measured sonar range and $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$0 the 3D sonar point in the world frame (Rahman et al., 2018). The dense-mapping paper extends this idea to tightly-coupled ultrasound imaging sonar fused as additional range factors when available, improving robustness in feature-poor zones (Wang et al., 2023).

A major later use of SVIn2 is as a front end for reconstruction. In the real-time dense 3D mapping system, high-rate SVIn2 pose output drives a CPU-only depth-estimation and fusion pipeline consisting of stereo pair to SAD cost volume, 4-path SGM, subpixel parabola fitting, a PKRN ratio test for depth confidence, sliding-window fusion of three consecutive depth maps, and projection of each fused map into a global cloud by the corresponding SVIn2 pose (Wang et al., 2023). The paper reports that the pipeline runs on a single Intel i7-10700K using 12 OpenMP threads, with IMU preintegration and visual tracking maintaining 15 Hz real-time operation, stereo cost-volume plus SGM taking approximately 300–400 ms per frame, fusion approximately 350–700 ms per frame, and effective end-to-end throughput of 2.8–10.2 Hz on underwater sequences (Wang et al., 2023).

In the cave-mapping workflow, SVIn2 is not itself the dense mapper. Rather, approximately 40 000 SVIn2 keyframes, consisting of camera poses and sparse points, are exported to COLMAP as fixed priors; COLMAP then runs a global bundle adjustment over all keyframes in a user-selected spatial window, jointly refining camera intrinsics, extrinsics, and 3D points, after which a final Poisson reconstruction yields a dense mesh (Chatzispyrou et al., 8 Jul 2025). This split pipeline is explicitly described as combining the robust locally optimized trajectory with photorealistic dense reconstruction (Chatzispyrou et al., 8 Jul 2025).

7. Empirical performance, applications, and limitations

The 2018 paper evaluates SVIn2 on EuRoC MAV and four underwater datasets. On EuRoC MAV Machine Hall 01–05, with stereo at 20 Hz and IMU at 200 Hz and without sonar or depth fusion, SVIn2 outperforms OKVIS in every sequence and is competitive with VINS-Mono (Rahman et al., 2018). On underwater datasets including Sunken Bus, Cavern1, Cavern2, and an underwater cemetery dataset, only SVIn2 maintained metric scale and completed all loops; VINS-Mono either lost track or produced wrong-scale trajectories; OKVIS drifted and failed to detect loops; and MSCKF frequently diverged (Rahman et al., 2018).

In "Real-Time Dense 3D Mapping of Underwater Environments," the trajectory accuracy of SVIn2 relative to COLMAP is reported by trajectory ATE (RMS): 0.07 m on Ginnie Ballroom at approximately 99 m, 0.19 m on Cenote at approximately 67 m, and 0.39 m on Coral Reef at approximately 45 m (Wang et al., 2023). Depth-map error with respect to COLMAP improves after fusion from median-of-medians 0.13/0.19/0.40 and MAE 0.45/0.47/0.63 to 0.06/0.11/0.33 and 0.08/0.13/0.40 across Ginnie, Cenote, and Reef, respectively (Wang et al., 2023). Final point-cloud Chamfer metrics are reported as precision $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$1, accuracy $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$2 m, recall $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$3, and completeness $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$4 m for Ginnie Ballroom; precision $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$5, accuracy $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$6 m, recall $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$7, and completeness $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$8 m for Cenote; and lower values for Coral Reef under more challenging conditions (Wang et al., 2023). The same paper reports that COLMAP sparse plus MVS on 1,500–3,500 keyframes takes 300–1,250 min total, approximately $x_i = [R_i, p_i, v_i, b_{a,i}, b_{\omega,i}],$9–$X_l$0 Hz, whereas the proposed pipeline runs in 20–23 min, approximately $X_l$1–$X_l$2 Hz, on a single CPU (Wang et al., 2023).

In "Mapping the Catacombs," SVIn2 is used on an approximately 1 km trajectory over 81 min of dive time (Chatzispyrou et al., 8 Jul 2025). After time-shift and depth-offset alignment, the SVIn2 z-curve matches the dive computer to less than 0.1 m RMS, and the average centerline from SVIn2 lies within the $X_l$3 m uncertainty of the MNemo V2 manual survey (Chatzispyrou et al., 8 Jul 2025). The paper further states that in narrow passages the sparse SVIn2 point cloud captures walls to within 0.1–0.2 m, while in wide, dimly lit areas dense reconstruction struggled, producing noisy distant geometry (Chatzispyrou et al., 8 Jul 2025).

The limitations reported across these papers are consistent. The original system notes that in extremely low-visibility regions where even CLAHE fails to reveal features, front-end tracking can still degrade, and that real-time performance on resource-constrained AUVs requires further code optimization and potential sparsification of the sonar term (Rahman et al., 2018). The cave-mapping work adds that dense meshing of the entire cave exceeded available memory, open high-ceiling passages provide too few features for stable map points, and no real-time dense map is yet available because the full dense pipeline is post-dive (Chatzispyrou et al., 8 Jul 2025). It also notes that no explicit refraction or water-turbidity modeling is used (Chatzispyrou et al., 8 Jul 2025). This suggests that SVIn2’s strongest regime is metric trajectory estimation and sparse-to-semidense structural support under difficult underwater sensing, while large-scale dense mapping remains coupled to computational and scene-structure constraints rather than being fully resolved within the core SLAM system itself.