- Object for storing 3-D point cloud
- 3-D rigid geometric transformation
- Transform 3-D point cloud
- Register two point clouds using ICP algorithm
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)
Show older comments
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');
0 Comments
Answers (1)
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:
0 Comments
See Also
Categories
Find more on Point Cloud Processing in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!