version 1.0.0.0 (13.4 KB) by
Dirk-Jan Kroon

Iterative Closest Point using finite difference optimization to register 3D point clouds affine.

This function ICP_FINITE is an kind of Iterative Closest Point(ICP) registration algorithm for 3D point clouds (like vertice data of meshes ) using finite difference methods.

Normal ICP solves translation and rotation with analytical equations. By using finite difference this function can also solve resizing and shear thus affine registration.

As first step, this function sorts the static points into a grid of overlapping blocks. The block nearest to a moving point will always contain its closest static point, thus the grid allows faster registration.

Dirk-Jan Kroon (2021). Finite Iterative Closest Point (https://www.mathworks.com/matlabcentral/fileexchange/24301-finite-iterative-closest-point), MATLAB Central File Exchange. Retrieved .

Created with
R2009a

Compatible with any release

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

Start Hunting!Create scripts with code, output, and formatted text in a single executable document.

Bahetihazi MaiduJing-Sheng LiEduardo SantosEduardo SantosSIMPLE DEMO:

Hi! For those who are looking for a DEMO, here is a simple DEMO based on another ICP implementation demo from Martin Kjer and Jakob Wilm, Technical University of Denmark, 2012:

% demo_ICP_finite.m

% ICP_finite - Iterative Closest Point FINITE

clc

clear

close all

m = 30; % width of grid

n = m^2; % number of points

[X,Y] = meshgrid(linspace(-2,2,m), linspace(-2,2,m));

X = reshape(X,1,[]);

Y = reshape(Y,1,[]);

Z = sin(X).*cos(Y);

% Create the data point-matrix

D = [X; Y; Z];

% Translation values (a.u.):

Tx = 0.5;

Ty = -0.3;

Tz = 0.2;

% Translation vector

T = [Tx; Ty; Tz];

% Rotation values (rad.):

rx = 0.3;

ry = -0.2;

rz = 0.05;

Rx = [1 0 0;

0 cos(rx) -sin(rx);

0 sin(rx) cos(rx)];

Ry = [cos(ry) 0 sin(ry);

0 1 0;

-sin(ry) 0 cos(ry)];

Rz = [cos(rz) -sin(rz) 0;

sin(rz) cos(rz) 0;

0 0 1];

% Rotation matrix

R = Rx*Ry*Rz;

% Transform data-matrix plus noise into model-matrix

M = R * D + repmat(T, 1, n);

% Add noise to model and data

rng(2912673);

M = M + 0.01*randn(3,n);

D = D + 0.01*randn(3,n);

% Run ICP FINITE (standard settings)

[Points_Moved,MH]=ICP_finite(M', D');

% [Points_Moved,MM]=ICP_finite(M, D, Options);

% Transform data-matrix using ICP FINITE result

Dicp = movepoints(MH, D');

Dicp = Dicp';

% Plot the results

% subplot(2,2,2);

plot3(M(1,:),M(2,:),M(3,:),'bo',Dicp(1,:),Dicp(2,:),Dicp(3,:),'ro');

% plot3(M(1,:),M(2,:),M(3,:),'bo',Dicp(1,:),Dicp(2,:),Dicp(3,:),'r.');

axis equal;

xlabel('x'); ylabel('y'); zlabel('z');

title('ICP result');

NilsHi,

i'd suggest replacing this section

" for i=1:size(Points_Moved,1)

% Find closest group point

Point=Points_Moved(i,:);

dist=(Points_Group(:,1)-Point(1)).^2+(Points_Group(:,2)-Point(2)).^2+(Points_Group(:,3)-Point(3)).^2;

[mindist,j]=min(dist);

% Find closest point in group

Points_Group_Static=Cell_Group_Static{j};

dist=(Points_Group_Static(:,1)-Point(1)).^2+(Points_Group_Static(:,2)-Point(2)).^2+(Points_Group_Static(:,3)-Point(3)).^2;

[mindist,j]=min(dist);

Points_Match(i,:)=Points_Group_Static(j,:);

end

"

by

these lines.

[idx]=knnsearch(Points_Static,Points_Moved);

Points_Match = Points_Static(idx,:);

It is much faster for me.

Ben GuhlThis is just gold. Thank you so much for this!

Ben Guhlso

raiyea有没有介绍相关原理的文献呢？？

kian motahariit does work perfectly,tnx.but i'm wondering whether i can attach the RGB matrix to each point,then the result would contains the color too.do you have any solution?

SarunDiniz Saworked as expected, tank you all!

Ahmed Yehialightol smithhow to cite your code?

Denis Stein@How to cite the work:

Use his PhD thesis, available at <http://doc.utwente.nl/78499/1/thesis_D_Kroon.pdf>. He references to here on page 149.

N Sto cite*

N SI would like to your to your code. can you please tell me how i can do it? thanks

FrancescoHi all, i'd like to know if anybody has tried to use this algorithm on very large datasets (like point clouds with 300000-500000 points) and which options are recommended to minimize calculation times.

thank you all

YoussefHey Dirk-Jan Kroon.

Thanks for the code! Somehow i cannot run it though. I get "Itteration Error" Can you help me find what's wrong?

both pointclouds I am using are taken from a kinect sensor and slightly different capture angles.

I would appreciate and pointers on that as I am new to this field!

thanks

N SHi Kroon, your code works perfectly for me. now I would like to add "kd tree" function as a matching method to this algorithm, could you please help me to do that? Regards, Nazila

MiltonYour ICP code works great and I am able to achieve very good registration of my point cloud.

The point cloud is generated by using the vertices of an isosurface of my 3D grayscale data set. I would like to use the affine transform matrix M to transform the 3D grayscale data in the same way as the point cloud of vertices were transformed.

For some reason this does not seem to work. I cannot get my dataset to rotate correctly when using the output affine transform matrix.

Could you post an example of how you can use M to rotate not a point cloud, but a grayscale 3D dataset? Or is there some reason that should not work?

Thanks for sharing your great work.

Stormsharkvery nice code. Easy to understand and apply to other platform

HeliaChrisgreat code. similarly, reference/citation information would be great.

ChrisgangLuziaHi, I've used your ICP successfully for one of my projects - I would like to put a reference in the paper I'm preparing, could you let me know how you'd like me to cite it?

BenCan it be used for registering 2D point sets?

AlexThis code works perfectly well! Thank you for your nice and really useful work

JK HwangHi, Dirk-Jan Kroon. I was wondering about your nearest points searching method in your code. What is the reason why you used a grid spacing term, 'spacing=size(Points_Static,1)^(1/6)*sqrt(3);', in your code when uniform grid of points is established. Is there any reference about that?

Dirk-Jan Kroon* Jacques Saade

The code is written from scratch and not published in a paper.

You can of course reference the original paper.

Meidya Koeshardiantonice code

Jacques SaadeHi, really great work! But me too I ask if you can put a reference for the work

RobertFixed, but a demo code would be a nice addition.

RobertI tried running this program, and all the points on my data model were transformed to a singular point. If this could be explained, I would revise my rating. For example, put a demo up, so I can see that it works for something you worked with.

yusorry,but i cannot run it successfully. the error is

''??? Error using ==> tic

Too many output arguments......."

btw, my database contains about 40 thousand 3-D points.

expecting your reply.

M.Could you please put a reference for this work? Thanks a lot for uploading it.