필터 지우기
필터 지우기

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.

조회 수: 3 (최근 30일)
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');

답변 (1개)

Shivam Singh
Shivam Singh 2022년 4월 4일
편집: Shivam Singh 2022년 4월 4일
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:

카테고리

Help CenterFile Exchange에서 Point Cloud Processing에 대해 자세히 알아보기

태그

Community Treasure Hunt

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

Start Hunting!

Translated by