Mapping, Planning and Exploration with Pose SLAM