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.

2 views (last 30 days)
Dineish Arumugam
Dineish Arumugam on 27 Mar 2022
Edited: Shivam Singh on 4 Apr 2022
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');

Answers (1)

Shivam Singh
Shivam Singh on 4 Apr 2022
Edited: Shivam Singh on 4 Apr 2022
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:

Community Treasure Hunt

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

Start Hunting!