ICP - Iterative Closest Point algorithm, c++ implementation. Handles only points in R^3. Makes use of a kd-tree for closest-point search. ICP finds the transformation of points in data to fit points in model. Fit with respect to minimize a weighted sum of squares for distances between the data points and the corresponding closest model points.
Yohai Devir C++ compiler complained about the source code. Yohai changed the code so it works.