As mentioned earlier, the strategy of ICP is to always use closest points. To speed up computation, dtrees have been proposed [3]. Dtrees 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 dtree. 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 depthfirst search, thus expensive ballwithinbounds 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 dtrees is used to search the point correspondences. For every color, i.e., semantic label, a separate search dtree 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 dtrees 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.
