A Complete Solution For Autonomous Bathymetry Mapping