6D slam with cached K-D tree search

Abstract

6D SLAM (Simultaneous Localization and Mapping) or 6D Concurrent Localization and Mapping of mobile robots considers six degrees of freedom for the robot pose, namely, the x, y and z coordinates and the roll, yaw and pitch angles. In previous work we presented our scan matching based 6D SLAM approach [10-12, 16], where scan matching is based on the well known iterative closest point (ICP) algorithm [3]. Efficient implementations of this algorithm are a result of a fast computation of closest points. The usual approach, i.e., using k-d trees is extended in this paper. We describe a novel search strategy, that leads to significant speed-ups. Our mapping system is real-time capable, i.e., 3D maps are computed using the resources of the used Kurt3D robotic hardware.