6D Slam with Cached K-D Tree Search

A. Nchter, K. Lingemann, and J. Hertzberg (Germany)


Simultaneous Localization and Mapping (SLAM), 3Dmapping, ICP algorithm, k-d tree search, laser scanner


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 clos est 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.

Important Links:

Go Back