File Exchange

image thumbnail

ICP Registration using Efficient Variants and Multi-Resolution Scheme

version (317 KB) by Tolga Birdal
A fast, robust ICP, which is designed to work in hard scenarios e.g. clutter or bad initializations


Updated 03 Dec 2015

View Version History

View License

This sample implements a very efficient and robust variant of the iterative closest point (ICP) algorithm. The task is to register a 3D model (or point cloud) against a set of noisy target data. The variants are put together by myself after certain tests. The task is to be able to match partial, noisy point clouds in cluttered scenes, quickly. You will find that my emphasis is on the performance, while retaining the accuracy.
There is some more text within the documentation file, which is provided along with the zip. Although, the documentation misses the references, they will put there after the update. I hope this would be useful to many, because despite the quantity of the papers, I was unable to find a quality implementation.
The main contributions come from:
1. Picky ICP:
2. Efficient variants of the ICP Algorithm:
3. Geometrically Stable Sampling for the ICP Algorithm:
4. Multi-resolution registration:
5. Linearization of Point-to-Plane metric by Kok Lim Low:
Note that the test function requires Gabriel Peyré's read_ply m-file in here:

Cite As

Tolga Birdal (2021). ICP Registration using Efficient Variants and Multi-Resolution Scheme (, MATLAB Central File Exchange. Retrieved .

Comments and Ratings (22)

Jason Wang

If it throw out the error "MATLAB:print:CannotCreateOutputFile 'c:/data/icp_ss_7.png' ", you can revise the path in the code "fn = ['c:/data/icp_ss_' num2str(level) '.png'];" and "fn = ['c:/data/icp_ss_' num2str(-1) '.png'];" in icp_mod_point_plane_pyr.m

Jason Wang

xiongce xiongce

read_ply can also be found here
a c++ version can be found in opencv contrib

xiongce xiongce

where is function read_ply ?
anyone can help me ?

Adrien Anxionnat

Great code, just a tiny recurrent error in some few cases in get_duplicates function, it is possible de correct it by the code bellow:

% Returns the duplicates of a vector
% Author: Tolga Birdal

function duplicates = get_duplicates(X)
%[uniqueX i j] = unique(X,'first');
% duplicates = 1:length(X);
% duplicates(i) = [];
%duplicates = find(not(ismember(1:numel(X),i)));
uniqueX = unique(X);
countOfX = hist(X,uniqueX);
index = (countOfX~=1 & countOfX~=0);

duplicates = uniqueX(index(:));
% indices to unique values in column 3
[~, ind] = unique(X);
% duplicate indices
duplicate_ind = setdiff(1:size(X, 1), ind);
% duplicate values
duplicates = unique(X(duplicate_ind));



young may

check_face_vertex function is missing. Please provide it. Thanks


check_face_vertex function is missing.

Shahab Askarian

check_face_vertex function is missing. Please provide it. Thanks


check_face_vertex function is missing. Pls provide it. Thnx

fegnxiang wang

junzhi guan

hi, what the meaning for SrcN or DstN in the input arguments?

I thought you just need two sets of 3d points.

Tolga Birdal

I added movepoints function.
FinalPose is the transformation returned.


Also movepoints() does not seem to have been supplied.


Is there a way to have the rigid transformation matrix (either in 3D with a translation or as a single homogeneous transformation matrix) be returned from the function?


@Tolga Birdal, Hi, which movepoints function you have used?


@Tolga Birdal, Hi, which movepoints function you have used?

roger wu

Dear Claudia,
can you provide code:ply_read,thanks!

BR roger

Tolga Birdal

Dear Claudia,

You are right. However, my ICP benefits from a multi-resolution scheme. This means that at each level of the pyramid, the error is recomputed and minimized. This way it can compensate up to ~25-30 degrees differences.

f_val records all the iterations, it is not per pyramid. For this reason you should be seeing a curve similar to e.g. a tangent curve (multiple peeks with decreasing fashion between intervals). The peaks mean that the algorithm just switched to a finer level. The ICP error is provably convergent and it cannot go up for no reason. This is why, I could recommend a temporary workaround: Detect the peeks and take the last one.

However, I take your suggestion into consideration and will change it accordingly.

Note that the final error depends on the problem. If you have many outliers / noise you will get a high value. If not you will get a very low one. This is an indication of convergence, not an indication of correct registration (higher errors do not always mean correct convergence). However, for a similar problem, expect similar values. This is valid for all ICP implementations.

Claudia Stoecker

Thanks a lot for submitting the script for ICP-registration. In my research, I want to test some different combination of parameter to get best alignment. Is there a variable in your script which is similar to an registration error respectively a RMS alignment error? Primarily I thought this is "fval" in icp_mod_point_plane_pyr.m , but I think, this is not the correct one - when I plot this value against number of iteration it looks a little bit strange.

Claudia Stoecker



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

Community Treasure Hunt

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

Start Hunting!