Towards Robot-Aided Diver Navigation in Mapped Environments (ROADMAP)