Pose interpolation SLAM for large maps using moving 3D sensors

Precise 3D mapping and 6DOF trajectory estimation using exteroceptive sensors are key problems in many fields. Real-time moving laser sensors gained popularity due to their precise depth measurements, high frame rate and large field of view. We propose an optimization framework for Simultaneous Localization And Mapping that properly models the acquisition process in a scanning-while-moving scenario. Each measurement is correctly reprojected in the map reference frame by considering a continuous time trajectory which is defined as the linear interpolation of a discrete set of control poses in SE3. The trajectory estimation is performed using the sensor readings only, i.e., no external motion measurement units are used. An efficient data structure that makes use of a hybrid sparse voxelized representation for large map management allows to perform global optimization over trajectories, resetting the accumulated drift when loops are detected. We experimentally show that such framework improves localization and mapping w.r.t. solutions that compensate the distortion effects without including them in the optimization step. Moreover, we show that the proposed map structure provides linear or constant time operations w.r.t. the map size in order to perform real time SLAM and it can handle very large maps.