6D SLAM with Cached kd-tree Search

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, where scan matching is based on the well known iterative closest point (ICP) algorithm [Besl 1992]. Efficient implementations of this algorithm are a result of a fast computation of closest points. The usual approach, i.e., using kd-trees is extended in this paper. We describe a novel search stategy, 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.