Iterative Closest Point

An implementation of various ICP (iterative closest point) features.
Updated 25 Jan 2013

View License

The ICP algorithm takes two point clouds as an input and return the rigid transformation (rotation matrix R and translation vector T), that best aligns the point clouds.

[R,T] = icp(q,p,10);

Aligns the points of p to the points q with 10 iterations of the algorithm.
The transformation is then applied using
R*p + repmat(T,1,length(p));

The file has implemented both point to point and point to plane as well as a couple of other features such as extrapolation, weighting functions, edge point rejection, etc.

For an introductory text on the ICP algorithm and the implemented variants, see

Hans Martin Kjer & Jakob Wilm -- Evaluation of surface registration algorithms for PET motion correction

Cite As

Jakob Wilm (2024). Iterative Closest Point (, MATLAB Central File Exchange. Retrieved .

MATLAB Release Compatibility
Created with R2012a
Compatible with any release
Platform Compatibility
Windows macOS Linux

Inspired: Optimal Step Nonrigid ICP

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!
Version Published Release Notes

Replaced the link to the thesis.

Modified Orthogonal Procrustes to prevent non-plausible rotation matrix (det(R) should be +1).
-- Thank you, Tomasz!

Clearing the input parser object due to memory leaks (in older Matlab versions?) -- thanks, Walter!

* Added a missing sub function: k_nearest_neighbors
* Fixed a bug in the lsqnorest sub function
* Removed GLTree reference because it is not available any longer
* Updated the examples to be clearer

Updated icp.m with a missing sub function.

Corrected link to thesis.

Combined functions in one file and added a demo script.

Corrected a typo in icp.m (as pointed out in the comments, thanks!)

Added rms_error and added a screenshot.

Added link to thesis.