Optimization of ICP Using K-D Tree

1. Optimization of ICP Using K-D Tree Baochun Bai Instructor: Prof. Boulanger

2. Outline • An Introduction to ICP • Algorithm Steps • KD-Tree • Method to compute rotation and translation matrix • Results

3. Overview of ICP Algorithm • A popular algorithm to register a data shape to a model shape • Data shape point set P{pi} with Np points • Model shape point set X{xi} with Nx points • Find the optimal rotation R and translation matrix T to transform P to X • Minimize the mean square error

4. Algorithm Steps of ICP • Initialize P0 = P, R0 = I, T0 = (0, 0, 0) • For each iteration k, • Find the closest points Yk = C(Pk, X) based on Euclidean distance • Compute the registration and find rotation matrix Rk and translation matrix Tk • Apply the registration, Pk+1 = Rk(Pk) + Tk • Stop if mean square error is less than a threshold

5. Nearest Neighbor Search Algorithms • Performance bottleneck of the algorithm • Straightforward Approach • Sequential Search • O(NpNx), Np <= Nx, O(N2) • K-D Tree • A binary search tree • O(logN)

6. K-D Tree • A balanced binary search tree for k-dimensional data • Root node: entire space • Leaf node: a mutually exclusive subspace • Intermediate node: key to partition the space, key is one of k dimensions

7. An Example of K-D Tree X Y

8. Algorithm to Compute Rotation and Translation Matrix • Compute the cross-covariance matrix of P and X • Compute the 4x4 symmetric matrix Q

9. Algorithm to Compute Rotation and Translation Matrix • Get the unit eigenvector qR of Q, which is corresponding to the maximum eigenvalue of Q • Compute the rotation matrix R from qR • Compute the translation matrix T based on R • T = ux - Rup

10. Results • Running Time • Bunny Model • 31000 points • 601MHZ, 128Mb Memory

11. Convergence of Mean Square Error