MBpIC-SLAM: Probabilistic Surface Matching for Bathymetry Based SLAM

This paper presents a probabilistic surface matching method for pose-based bathymetry SLAM using a multibeam sonar profiler. The proposed algorithm compounds swath profiles of the seafloor with dead reckoning localization to build surface patches. Accurately dealing with the uncertainty of the robot pose as well as the measured points, a Monte Carlo method is used for coarse registration before performing a fine registration using a probabilistic ICP 3D implementation. Moreover, using the proposed uncertainty estimation, an algorithmic improvement is also presented to decrease the complexity of the ICP to O(n). The method has been tested using an experimental dataset gathered in Tasmanian waters.