Collaborative Mapping and Autonomous Parking for Multi-story Parking Garage