Probabilistic surface matching for bathymetry based SLAM

This paper describes 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. Then, a probabilistic implementation of the ICP is used to deal with the uncertainty of the robot pose as well as the measured points in a two-stage process including point-to-point and point-to-plane metrics. A novel surface adaptation using octrees is proposed to have ICP-derived methods working in feature-poor or highly unstructured areas typical of bathymetric scenarios. Moreover, a heuristic based on the uncertainties of the surface points is used to improve the basic algorithm, decreasing the ICP complexity to O(n). The performance of the method is demonstrated with real data from a bathymetric survey.