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/spl I.oarr//sub i//spl isin/P that neighborhood of points which lie within a certain distance /spl epsiv/ of p/spl I.oarr//sub i/. The points within each /spl epsiv/-neighborhood are sorted by increasing distance to their respective p/spl I.oarr//sub 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 satifies a constraint, called the Spherical Constraint, then the nearest neighbor falls within the /spl epsiv/-neighborhood of the estimate. A novel theorem, the Ordering Theorem, is presented which allows the Triangle Inequality to efficiently prune points from the sorted /spl epsiv/-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 /spl sim/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.