aqua_localization
ROS 2 underwater localization for real AUV/ROV data.
Fuse IMU, pressure, DVL, sonar registration, and pose-graph loop closure
into a stack built around public ocean datasets.
Ocean localization is hard in specific ways: GNSS disappears at the surface,
pressure becomes the primary depth measurement, DVL arrives in the vehicle
frame, and sonar returns are sparse or geometrically ambiguous. This stack
keeps those underwater-specific paths explicit and testable — a 15-state
additive UKF, pressure-depth and DVL-velocity updates, PCL-based sonar
registration, a g2o SE(3) pose-graph backend, and an experimental MBES
submap loop-closure front end. The headline results are public-data first:
four real underwater datasets, four rerun.io renderings,
no synthetic-bag or simulator-only demo. Targets BlueROV2-class ROVs, custom
AUVs, and uuv_simulator, on ROS 2 Humble and Jazzy.
Latest release: v0.5.
If this saves bring-up time or gives your AUV/ROV project a useful baseline,
a ⭐ helps other underwater-robotics users find it — the public goal is
10 GitHub stars through reproducible demos and honest limitations.
Public-Data Results
Each row has a one-shot recorder and rerun export script, so the images below
are generated from recorded demo bags rather than hand-captured screenshots.
| Dataset |
What it shows |
rerun view |
Tank Dataset short_test |
Tightly-coupled aqua_graph (stereo + IMU + DVL + pressure) 0.0092 m APE RMSE on AQUA-SLAM’s 11.65 s window (AQUA-SLAM: 0.0194 m); UKF 0.137 m. Single public sequence, not yet held-out (details) |
tank_dataset_rerun.png |
MBES-SLAM beach_pond |
Multibeam fans accumulated into a depth-coloured bathymetric scan |
mbes_slam_rerun.png |
NTNU subset-fjord/fjord_1 |
Dataset SLAM baseline through a 7 m fjord dive |
ntnu_fjord_1_rerun.png |
AQUALOC harbor_07 |
LIRMM “Dumbo” ROV underwater camera + pressure depth track |
aqualoc_harbor_07_rerun.png |
Start Here
What It Does
| Signal |
Why it matters underwater |
| Pressure depth |
Keeps the vertical axis observable below the surface. |
| DVL velocity |
Provides vehicle-frame motion constraints for AUV/ROV dead reckoning. |
| Stereo camera |
Experimental ORB + PnP visual odometry for Tank-style underwater stereo bags. |
| Multibeam sonar |
Turns acoustic fans into bathymetric structure for registration. |
| Pose graph edges |
Keeps long seafloor replays inspectable through keyframes and loop constraints. |
| rerun / RViz views |
Shows trajectories, sonar clouds, loop candidates, and tuning diagnostics. |
Underwater 3DGS Sample Pack
The first public 3DGS input artifact is a small Tank Dataset short_test pack
for quick inspection and external reconstruction experiments: 20 PNG frames, 20
matched /apriltag_slam/GT poses, manual Tank stereo intrinsics, and a
nerfstudio-style transforms.json. The readiness gate passes with
check_3dgs_training_ready.py. This is an input-pack artifact, not a trained
Gaussian Splatting result.
Run It
Build
# Clone into a colcon workspace, install dependencies, build, and source.
git clone https://github.com/rsasaki0109/aqua_localization.git
cd aqua_localization && rosdep install --from-paths . --ignore-src -r -y
cd ..
colcon build --symlink-install
source install/setup.bash
# Launch the full stack with default parameters.
ros2 launch aqua_localization aqua_localization.launch.py
Public Demo
The smallest validated path is the Tank Dataset short_test sequence: about
15 seconds of real underwater motion with IMU, pressure-derived depth, DVL,
and AprilTag ground truth.
# After following datasets/tank_dataset_demo.md to download and convert
# short_test.bag, record the estimator outputs into a self-contained demo bag.
ros2 run aqua_localization record_tank_demo.sh
# Export to rerun.io for a browser-friendly 3D + plots replay.
ros2 run aqua_localization rerun_export.py \
--bag aqua_localization/datasets/public/tank_dataset/demo_with_estimate \
--out /tmp/tank.rrd
rerun /tmp/tank.rrd
Per-dataset bring-up notes (download size, conversion, calibration, replay):
tank ·
mbes_slam ·
beach_pond acquisition ·
ntnu ·
aqualoc.
Loop-Closure Smoke Test
No bag is needed for the pose-graph loop-closure smoke path. This starts the
g2o backend, publishes a tiny odometry chain plus one loop constraint, and
checks that the graph accepted both the keyframes and the loop edge:
ros2 run aqua_localization pose_graph_loop_smoke.sh
The same lower-level publisher is available as
ros2 run aqua_localization pose_graph_loop_demo.py when you want to launch
aqua_pose_graph yourself and inspect /aqua_pose_graph/path.
Stack Map
| Package |
Role |
aqua_imu_loc |
15-state additive UKF for IMU, pressure, DVL, and sonar position updates |
aqua_sonar_loc |
PointCloud2 preprocessing and PCL ICP/GICP/NDT registration with a submap front end |
aqua_fusion |
Loose-coupling fusion of IMU/depth and sonar odometry |
aqua_pose_graph |
g2o SE(3) keyframe graph with external loop-constraint input |
aqua_graph |
Experimental GTSAM fixed-lag smoother: IMU preintegration + DVL + pressure (+ position) factors in one optimizer |
aqua_replay |
Deterministic offline replay: runs the nodes as components in one process, bag fed in stamp order, bit-identical repeat runs |
aqua_msgs |
Diagnostic and fusion-input message types |
aqua_localization |
Metapackage, top-level launches, dataset scripts, and replay exports |
Detailed architecture per package: docs/architecture.md.
Experimental MBES Loop Closure
The MBES path includes an experimental submap-vs-submap loop-closure front end.
It accumulates bathymetric submaps between pose-graph keyframes, tests
odometry-near historical candidates with ICP/GICP/NDT, publishes accepted
constraints to /aqua_pose_graph/loop_constraint, and exposes tuning
diagnostics on /mbes_loop_closure/status plus RViz markers on
/mbes_loop_closure/markers.
Use the dedicated RViz view while tuning real bags:
ros2 launch aqua_localization replay.launch.py \
start_bag:=true \
bag_path:=aqua_localization/datasets/public/mbes_slam/beach_pond_ros2 \
bag_sonar_points_topic:=/norbit/detections \
use_sim_time:=true \
enable_pose_graph:=true \
enable_mbes_loop_closure:=true \
enable_rviz:=true \
rviz_config_file:=$(ros2 pkg prefix aqua_localization)/share/aqua_localization/rviz/mbes_loop_closure.rviz
This front end is not yet production-tuned; the next reliability step is
real-bag status export, threshold sweeps, and false-positive analysis.
Visualization Exports
Two browser-friendly paths run on the same self-contained demo bag:
Bag-recording recipe: docs/foxglove/README.md.
Project Status
The headline next milestone is reliable real-data loop closure. The
pose-graph backend and an experimental MBES front end are in place; the
remaining work is threshold tuning, candidate reliability, information-matrix
calibration, and eventually visual loop closure for AQUALOC. An ESKF backend,
magnetometer fusion, and acoustic positioning are also on the list. The nearest
concrete steps are a 3DGS training smoke test on the published Tank pack, an
MBES threshold sweep turning loop-closure tuning into accepted/rejected
summaries, and a reproducible AQUA-SLAM comparison run.
Planning and validation references: PLAN.md ·
mvp_checklist ·
benchmarks/ ·
real_bag_evaluation ·
sota_gap_analysis ·
oss_comparison ·
aqua_slam_comparison ·
mbes_loop_closure ·
public_launch_checklist.
Honest Limitations
- IMU-only dead reckoning drifts roughly an order of magnitude on bags without
DVL/visual aiding (NTNU
fjord_1, AQUALOC harbor_07 show hundreds of meters
of XY drift; depth z(t) tracks well via the pressure update).
- Single-fan multibeam registration is geometrically degenerate.
Tightly-coupled sonar feedback narrows MBES-SLAM
beach_pond fusion drift
from ±40 m to ~17 m, but per-fan residuals are still ~10 m magnitude. The
loop-closure thresholds and information matrix are not yet calibrated on a
full real-bag tuning run.
aqua_fusion has unit + runtime tests but no per-platform benchmark history
yet.
Testing
colcon test --packages-select \
aqua_imu_loc aqua_sonar_loc aqua_fusion aqua_pose_graph aqua_graph aqua_replay \
aqua_localization --event-handlers console_direct+
Run colcon test-result --verbose after testing for the current count; the
latest local Jazzy validation (2026-10-10) reported 722 tests with zero
failures. CI runs the same build and tests on Humble and Jazzy.
For repeatable experiments, replay bags through the deterministic harness
instead of ros2 bag play (see aqua_replay/scenarios):
ros2 run aqua_replay offline_replay --scenario <scenario.yaml> --bag <in> --out <out>
ros2 run aqua_replay compare_replay_summaries.py <out_a> <out_b> # repeatability
Contributing
Bug reports, dataset bring-up notes, benchmark results, and focused pull
requests are welcome. Start with CONTRIBUTING.md for the
expected build/test commands and issue templates.
License
Apache-2.0. See LICENSE and individual package.xml files for
per-package maintainer info.
评论