A nearest neighbor method for efficient ICP

A novel solution is presented to the Nearest Neighbor Problem that is specifically tailored for determining correspondences within the Iterative Closest Point Algorithm. The reference point set P is preprocessed by calculating for each point p ($) over right arrow (i) epsilon P that neighborhood of points which lie within a certain distance epsilon of p ($) over right arrow (i). The points within each epsilon -neighborhood are sorted by increasing distance to their respective p ($) over right arrow (i). At runtime, the correspondences are tracked across iterations, and the previous correspondence is used as an estimate of the current correspondence. If the estimate satisfies a constraint, called the Spherical Constraint, then the nearest neighbor falls within the epsilon -neighborhood of the estimate. A novel theorem, the Ordering Theorem, is presented which allows the Triangle Inequality to efficiently prune points from the sorted epsilon -neighborhood from further consideration. The method has been implemented and is demonstrated to be more efficient than both the k-d tree and Elias methods. After similar to 40 iterations, fewer than 2 distance calculations were required on average per correspondence, which is close to the theoretical minimum of 1. Furthermore, after 20 iterations the time expense per iteration was demonstrated to be negligibly more than simply looping through the points.