Highly accurate maximum likelihood laser mapping by jointly optimizing laser points and robot poses

In this paper we describe an algorithm for learning highly accurate laser-based maps that treats the overall mapping problem as a joint optimization problem over robot poses and laser points. We assume that a laser range finder senses points sampled from a regular surface and we utilize an improved likelihood function that accounts for two phenomena affecting the laser measurements that are often neglected: the conic shape of the laser beam and the incidence angle. To solve the entire problem we apply an optimization procedure that jointly adjusts the position of all the robot poses and all points in the scans. As a result, we obtain highly accurate maps. We evaluated our approach using simulated and real-world data and we show that utilizing the estimated maps greatly improves the localization accuracy of robots. The results furthermore suggest that the accuracy of the resulting map can exceed the resolution of the laser sensors used.