Hello. I have point cloud registration model and I want to register the point cloud Q to point cloud P. How can I do that by using iterative closest point (ICP)? Thank you.

mtclear all

clc

initialPoints = [ 0 0 0;

15 15 0;

30 15 15;

30 15 30;

40 15 45];

anglexx = 0.0;

angleyy = 0.0;

anglezz = 0.0;

rxx = [ 1 0 0;

0 cos(anglexx) -sin(anglexx);

0 sin(anglexx) cos(anglexx)];

ryy = [ cos(angleyy) 0 sin(angleyy);

0 1 0;

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

rzz = [ cos(anglezz) -sin(anglezz) 0;

sin(anglezz) cos(anglezz) 0;

0 0 1];

rotate1 = initialPoints * rxx;

rotate2 = rotate1 * ryy;

rotate3 = rotate2 * rzz;

f1 = figure;

plot3(initialPoints(:, 1), initialPoints(:, 2), initialPoints(:, 3), 'b.', 'MarkerSize', 30);

hold on;

%rotatedPoints = initialPoints * rotationMatrix

plot3(rotate3(:, 1), rotate3(:, 2), rotate3(:, 3), 'r.', 'MarkerSize', 30);

grid on;

xlabel('X', 'FontSize', 20);

ylabel('Y', 'FontSize', 20);

zlabel('Z', 'FontSize', 20);

%caption = sprintf('ERROR = %.2f ', MSE);

%title(caption, 'FontSize', 20);

legend('Initial', 'Rotated');

%SEARCH

p1=rand;

p2=rand;

p3=rand;

iteration=100000;

y=iteration;

MSE=zeros(iteration,1);

fit=zeros(iteration,1);

fit(1,1)=100000;

truefitness(1,1)=100000;

truefitness = zeros(iteration,12);

truefitness(1,1)=100000;

xbest=zeros(iteration,3);

xbestfit=zeros(iteration,3);

y=1;

while y < iteration

xbestfit(y,1)=100000;

y=y+1;

end

%initialized agent

y=1

x1min=-2*pi;

x1max=2*pi;

x2min=-2*pi;

x2max=2*pi;

x3min=-2*pi;

x3max=2*pi;

delta01=0.5;

delta02=0.5;

delta03=0.5;

x1=x1min+(x1max-x1min)*rand;

x2=x2min+(x2max-x2min)*rand;

x3=x3min+(x3max-x3min)*rand;

xbest(y,1)=x1;

xbest(y,2)=x2;

xbest(y,3)=x3;

y = 2;

while y < iteration+1

%fitness calculation

anglex = x1;

angley = x2;

anglez = x3;

rx = [ 1 0 0;

0 cos(anglex) -sin(anglex);

0 sin(anglex) cos(anglex)];

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

0 1 0;

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

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

sin(anglez) cos(anglez) 0;

0 0 1];

rotate4 = rotate3 * rx

rotate5 = rotate4 * ry

rotate6 = rotate5 * rz

error = 0;

i=1;

while i < 6

j=1;

while j < 4

error = error + (rotate6(i,j)-initialPoints(i,j))^2

j = j+1;

end

i = i+1;

end

MSE = error^0.5

if MSE < xbestfit(y-1,1)

xbest(y,1)=x1;

xbest(y,2)=x2;

xbest(y,3)=x3;

xbestfit(y,1) = MSE;

else

xbest(y,1)=xbest(y-1,1);

xbest(y,2)=xbest(y-1,2);

xbest(y,3)=xbest(y-1,3);

xbestfit(y,1) = xbestfit(y-1,1);

end

delta1=exp(5*y/iteration)*delta01;

delta2=exp(5*y/iteration)*delta02;

delta3=exp(5*y/iteration)*delta03;

x1predict=(xbest(y,1)-delta01)+((xbest(y,1)+delta01)-(xbest(y,1)-delta01))*rand;

x2predict=(xbest(y,2)-delta02)+((xbest(y,2)+delta02)-(xbest(y,2)-delta02))*rand;

x3predict=(xbest(y,3)-delta03)+((xbest(y,3)+delta03)-(xbest(y,3)-delta03))*rand;

p1=p1+rand;

p2=p2+rand;

p3=p3+rand;

z1=x1predict+sin(rand*2*pi)*abs(x1predict-xbest(y,1));

z2=x2predict+sin(rand*2*pi)*abs(x2predict-xbest(y,2));

z3=x3predict+sin(rand*2*pi)*abs(x3predict-xbest(y,3));

k1=p1/(p1+rand);

k2=p2/(p2+rand);

k3=p3/(p3+rand);

x1=x1predict+k1*(z1-x1predict);

x2=x2predict+k2*(z2-x2predict);

x3=x3predict+k3*(z3-x3predict);

if x1<-2*pi

x1=x1min+(x1max-x1min)*rand;

else

end

if x1>2*pi

x1=x1min+(x1max-x1min)*rand;

else

end

if x2<-2*pi

x2=x2min+(x2max-x2min)*rand;

else

end

if x2>2*pi

x2=x2min+(x2max-x2min)*rand;

else

end

if x3<-2*pi

x3=x3min+(x3max-x3min)*rand;

else

end

if x3>2*pi

x3=x3min+(x3max-x3min)*rand;

else

end

y = y+1;

end

anglex = xbest(iteration,1);

angley = xbest(iteration,2);

anglez = xbest(iteration,3);

rx = [ 1 0 0;

0 cos(anglex) -sin(anglex);

0 sin(anglex) cos(anglex)];

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

0 1 0;

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

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

sin(anglez) cos(anglez) 0;

0 0 1];

rotate4 = rotate3 * rx

rotate5 = rotate4 * ry

rotate6 = rotate5 * rz

error = 0;

i=1;

while i < 6

j=1;

while j < 4

error = error + (rotate6(i,j)-initialPoints(i,j))^2

j = j+1;

end

i = i+1;

end

MSE = error^0.5

f2 = figure;

plot3(initialPoints(:, 1), initialPoints(:, 2), initialPoints(:, 3), 'b.', 'MarkerSize', 30);

hold on;

%rotatedPoints = initialPoints * rotationMatrix

plot3(rotate6(:, 1), rotate6(:, 2), rotate6(:, 3), 'r.', 'MarkerSize', 30);

grid on;

xlabel('X', 'FontSize', 20);

ylabel('Y', 'FontSize', 20);

zlabel('Z', 'FontSize', 20);

caption = sprintf('ERROR = %.2f ', MSE);

title(caption, 'FontSize', 20);

legend('Initial', 'Rotated');

Answer 1

[Shivam Singh]

Hello Dineish,

It is my understanding that you want to register two-point cloud using iterative closest point (ICP) registration.

For doing so, you may use the following link for reference:

• Object for storing 3-D point cloud
• 3-D rigid geometric transformation
• Transform 3-D point cloud
• Register two point clouds using ICP algorithm