A Model-Based Approach to 3-D Imaging and Mapping Underwater