- Register two point clouds using ICP algorithm
- Register two point clouds using NDT algorithm
- Register two point clouds using phase correlation
- Register two point clouds using CPD algorithm
- Register two point clouds using LOAM 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 krill herd algorithm (KH)? Thank you.
2 vues (au cours des 30 derniers jours)
Afficher commentaires plus anciens
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 commentaires
Réponses (1)
Shivam Singh
le 4 Avr 2022
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:
0 commentaires
Voir également
Catégories
En savoir plus sur Point Cloud Processing dans Help Center et File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!