Robot localization and 3D mapping