This paper has presented a new solution to the simultaneous localization and mapping (SLAM) problem with six degrees of freedom. The method is based on the ICP scan matching algorithm. The paper investigates approximate data association using d-trees and bd-trees. d-trees empirically outperfom bd-trees with and without approximation. Approximation does not significantly deteriorate the quality of scan registration. From that we know of no other superior approach, scan matching based on reduced point sets and approximate d-trees is currently the most performant method at hand for scan matching in 6D SLAM.