Efficient 3D Segmentation, Registration and Mapping for Mobile Robots