6D SLAM with Cached kd-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, 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.