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 krill herd algorithm (KH)? Thank you.

조회 수: 1(최근 30일)
Dineish Arumugam 2022년 3월 29일
답변: Shivam Singh 2022년 4월 4일
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 2022년 4월 4일
Hello Dineish,
It is my understanding that you want to use Krill Herd (KH) algorithm for registering two-point clouds.
Currently, we are not supporting any direct function for the same. I have brought this issue to the notice of the concerned people, and it might be considered for a future release. You may use the following link for KH algorithm implementation reference:
You may also refer other point cloud registration algorithms available:

댓글을 달려면 로그인하십시오.

범주

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!

Translated by