-
Notifications
You must be signed in to change notification settings - Fork 0
/
Cyl_modelling2.m
157 lines (123 loc) · 5.24 KB
/
Cyl_modelling2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
function [Solution]=Cyl_modelling2
clc
path='/home/jcl/Matlab2017b/DE_cylinder_modelling';
holes_dir=dir([path '/*.pcd']);
holes=size(holes_dir,1)/6
%Plot each hole cloud
for i=1:holes
cloudfile=strcat('roi', num2str(i));
cloudfile=strcat(cloudfile, '.pcd');
nube = pcread(cloudfile);
pcshow(nube, 'MarkerSize', 25)
xlabel('X')
ylabel('Y')
zlabel('Z')
hold on
end
NP=input('\ \n Introduce population number: \n');
if isempty(NP)
NP=100;
fprintf(1,'\n \t Population 100 by default. \n');
end
NP=round(NP);
fprintf(1,'\n Population size: %i \n',NP);
% Genetic algorithm upper iterations limit
iter_max=input('\ \n Introduzce the maximum number of iterations: \ \');
iter_max=round(iter_max);
if isempty(iter_max)
iter_max= 40;
fprintf(1,'\n \t The default iterations are %d \n',iter_max);
end
%--------------------------------------------------------------------------
% Different options for the GL algorithm can be selected via keyboard:
% - DE Core Options:
% 1) Random Mutation, with Thresholding and Discarding (Default).
% 2) Basic version, Random Mutation, without Thresholding, Discarding.
% 3) Mutation from Best candidate, with Thresholding and Discarding.
% 4) Random Mutation, with Thresholding and Discarding, NP is
% drastically reduced (tracking) after convergence.
version_de=input('\ \n Introduce the DE version that you want to apply: \n 1) Random Mutation, with Thresholding and Discarding. \n 2) Basic version, Random Mutation, without Thresholding, Discarding. \n 3) Mutation from Best candidate, with Thresholding and Discarding. \n 4) Random Mutation, with Thresholding and Discarding, NP reduced (tracking) after convergence. \n');
if isempty(version_de)
version_de=1;
fprintf(1,'\n \t Option 1 by default. \n');
end
centers=csvread('centers.csv');
%Evaluate each hole cloud
for i=1:holes
center=centers(i,:)
cloudfile=strcat('roi', num2str(i));
cloudfile=strcat(cloudfile, '.pcd')
nube = pcread(cloudfile);
Points=nube.Count;
if Points > 5
fprintf(1,'\n Hole points considered: %i \n',Points);
xyz=nube.Location;
xyz=reshape(xyz, [1 size(xyz,1)*size(xyz,2)]);
xyz(isnan(xyz))=[];
sz=size(xyz,2);
for j=1:1:sz/3
x=xyz(j);
y=xyz(j+sz/3);
z=xyz(j+2*sz/3);
mat(ceil(j),:) = [x y z];
end
ptCloud=pointCloud(mat);
%--------------------------------------------------------------------------
%Initialization parameters:
D=7; % DE Number of Chromosomes.
F=0.8; % Differential variations factor (mutation)
CR=0.5; % Crossover constant
% Population parameter boundaries,[x,y,z,u,v,w]: xyz center point, uvw axis vector
%vector_min=[-1 -1 -1];
vector_min=[-0.4 -0.4 0.6];
%vector_max=[1 1 1 ];
vector_max=[0.4 0.4 1];
min=[center(1)-0.05, center(2)-0.05, ptCloud.ZLimits(1)-0.01, vector_min(1), vector_min(2), vector_min(3),0.0125]; %minimum an maximum
max=[center(1)+0.05, center(2)+0.05, ptCloud.ZLimits(1)+0.01, vector_max(1), vector_max(2), vector_max(3),0.03];
N_SIMULATIONS=1;
Solution.best_estimate=zeros(N_SIMULATIONS,D);
Solution.error=zeros(N_SIMULATIONS,1);
for simul=1:N_SIMULATIONS
% The initial population is randomly generated.
population=initiate_pop(min,max,NP,D);
%--------------------------------------------------------------------------
% The robot motion simulation starts. In a single step, the robot tries to
% locate itself. After convergence, robot motion is allowed until an 'f'
% is introduced in dir_disp. In this case, the GL module ends its
% execution.
fprintf(1,'\n Simulation: %d/%d ',simul,N_SIMULATIONS);
tic
% The DE-based GL filter is called
[bestmem,error,population]=alg_genet2(ptCloud,version_de,population,iter_max,max,min,NP,D,F,CR);
toc
vector=[bestmem(5) bestmem(6) bestmem(7)];
center=[bestmem(2) bestmem(3) bestmem(4)];
radius=bestmem(8);
avg=ev(ptCloud,vector,center,radius);
fprintf(1,'\n Estimated center by the GL filter (x, y, z) %f, %f, %f\n',bestmem(2),bestmem(3),bestmem(4));
fprintf(1,'\n Estimated axis vector by the GL filter (u, v, w) %f, %f, %f\n',bestmem(5),bestmem(6),bestmem(7));
u=[0 0 1];
angle_deg = atan2d(norm(cross(u,vector)),dot(u,vector));
fprintf(1,'\n Angle with sensor: %f deg \n',angle_deg);
fprintf(1,'\n Radius %f cms\n',bestmem(8)*100);
fprintf(1,'\n Error Radius: %f cms\n',abs(bestmem(8)*100-1.75));
fprintf(1,'\n Avg. Distance from cloud to cylinder: %f cms \n',avg*100);
% The best solution and the error are returned.
Solution.best_estimate(simul,:)=bestmem(2:(D+1));
Solution.error(simul)=error;
end
%--------------------------------------------------------------------------
% Representation of results
height=ptCloud.ZLimits(2)-ptCloud.ZLimits(1);
params=[Solution.best_estimate(1),Solution.best_estimate(2),Solution.best_estimate(3),Solution.best_estimate(1)+height*Solution.best_estimate(4),Solution.best_estimate(2)+height*Solution.best_estimate(5),Solution.best_estimate(3)+height*Solution.best_estimate(6),Solution.best_estimate(7)];
model = cylinderModel(params);
%quiver3(Solution.best_estimate(2),Solution.best_estimate(3),Solution.best_estimate(4),Solution.best_estimate(5),Solution.best_estimate(6),Solution.best_estimate(7))
if avg*100<3
plot(model)
else
end
else
fprintf(1,'\n More points needed to analyze the hole');
end
end
end