As mentioned earlier, the strategy of ICP is to always use closest points. To speed up computation, d-trees have been proposed . D-trees are a generalization of binary search trees. Every node represents a partition of a point set to the two successor nodes. For searching points we use optimized, approximate d-tree. The idea behind this is to return as an approximate nearest neighbor the closest point in the bucket region where the query point lies. This value is determined from the depth-first search, thus expensive ball-within-bounds tests and backtracking are not used. Here, optimization means to choose the split axis during construction perpendicular to the longest axis to minimize the amount of backtracking.
A forest of d-trees is used to search the point correspondences. For every color, i.e., semantic label, a separate search d-tree is created. The algorithm computes point correspondences according to the color. E.g., points belonging to the wall are paired with wall points of previous 3D scans. Fig. shows the point correspondences in case of semantic based matching (top) in comparison with normal closest point matching (bottom). The points at the change of colors are paired differently. Fig. shows extracted slices of the d-trees for the colors red and yellow.
Using semantic information helps to identify the correct correspondences, thus the number of ICP iterations for reaching a minimum is reduced. In addition, maximizing the number of correct point pairs guides the ICP algorithm to the correct (local) minimum leading to a more robost algorithm.