InsSo3D: Inertial Navigation System and 3D Sonar SLAM for turbid environment inspection

TL;DR

InsSo3D tackles the challenge of large-scale underwater SLAM in turbid conditions by fusing a 3D sonar with an INS to achieve accurate 6-DOF localization and mapping. The approach combines a frontend that builds sub-maps through frame-to-frame and frame-to-sub-map registrations with a backend pose-graph optimization that detects loop closures and merges sub-maps into a global TSDF map. Key innovations include a 3D sonar-specific registration framework (CFEAR-based oriented surfaces and GICP), TSDF space-carving for real-time frontend updates, and a global TSDF with dynamic re-integration of moved sub-maps. Field experiments in a test tank and an outdoor quarry demonstrate trajectory errors below $0.21\mathrm{m}$ and map reconstructions with mean errors around $0.09\mathrm{m}$ over tens of meters, validating InsSo3D for safe, large-scale underwater inspection in challenging visibility conditions.

Abstract

This paper presents InsSo3D, an accurate and efficient method for large-scale 3D Simultaneous Localisation and Mapping (SLAM) using a 3D Sonar and an Inertial Navigation System (INS). Unlike traditional sonar, which produces 2D images containing range and azimuth information but lacks elevation information, 3D Sonar produces a 3D point cloud, which therefore does not suffer from elevation ambiguity. We introduce a robust and modern SLAM framework adapted to the 3D Sonar data using INS as prior, detecting loop closure and performing pose graph optimisation. We evaluated InsSo3D performance inside a test tank with access to ground truth data and in an outdoor flooded quarry. Comparisons to reference trajectories and maps obtained from an underwater motion tracking system and visual Structure From Motion (SFM) demonstrate that InsSo3D efficiently corrects odometry drift. The average trajectory error is below 21cm during a 50-minute-long mission, producing a map of 10m by 20m with a 9cm average reconstruction error, enabling safe inspection of natural or artificial underwater structures even in murky water conditions.

Figures (10)

• Figure 1: InsSo3D uses the 3D Sonar and the INS to estimate the robot's trajectory and generate a 3D map of its environment. The trajectory colour indicates the acquisition timestamp of the 3D Sonar frame.
• Figure 2: Comparison between (a) Camera image, (b) 3D Sonar depth image, and (c) Camera depth image generated with RoMa edstedt2024roma thanks to excellent visibility conditions. The red rectangle indicates the overlapping area between 3D Sonar and camera FOV, and the pink ellipse highlights a similar feature. The range colour scale is indicated on the right. Also, the camera FOV is 60° by 45°, and the 3D Sonar FOV is 90° by 40°.
• Figure 3: Algorithm overview: new 3D Sonar frames are registered to the current sub-map using frame-to-frame and frame-to-sub-map registration. Then, a graph optimisation within the sub-map is performed to fuse the two registrations and estimate the correct frame pose. Finally, the new frame is integrated into the sub-map TSDF. When the submap completion criterion is met, the submap is closed and sent to the backend. The backend will register it to the previous one and try to detect loop closures with older sub-maps contained in the sub-map database (DB) before including the new sub-map in the factor graph. Finally, a TSDF representation of the global map is generated using each sub-map TSDF representation and the sub-map poses optimised by the factor graph.
• Figure 4: Process of computing the CFEAR oriented surface point cloud. (a) The point cloud is inserted into a voxel grid of voxel size $r$. (b) The mean and covariance are computed for each voxel using points within a distance $r$ of the voxel centroid. (c) Final representation.
• Figure 5: Factor graph representation of the frontend, red nodes are the frame's pose, blue nodes are sequential frame-to-frame factors, and the yellow nodes are frame-to-sub-map factors. Finally, the green node is a prior factor.