-
Notifications
You must be signed in to change notification settings - Fork 1
/
loadDatasetNao.m
222 lines (213 loc) · 10.5 KB
/
loadDatasetNao.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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
function datasets = loadDatasetNao(robot,optim, chains, varargin)
% LOADDATASETNAO returns 'datasets' and 'indexes' for given
% optimization settings.
% INPUT - robot - instance of @robot class
% - optim - structure with optimization settings
% - chains - structure with optimized chains
% - datasetNames - 1xN cell array with names of the
% datasets, e.g. {'rightArm_torso'}
% OUTPUT - datasets - 1x4 ([self-touch, planes, external, projection])
% cell array of cell arrays (1xN) of datasets
varargin=varargin{1};
if length(varargin)==1
alt=0;
datasetsNames=varargin;
else
if strfind(varargin{end},'Alt')
alt=1;
datasetsNames=varargin{1:end-1};
else
alt=0;
datasetsNames=varargin;
end
end
assert(~alt, 'Alternative mode is not supported now!');
%datasetsNames=varargin{1};
% Assign robot's kinematics into new variable
kinematics=robot.structure.kinematics;
if size(kinematics.leftArm,2)==4
[kinematics, ~] = padVectors(kinematics);
end
% Add 'dummy' torso link to precompute RT matrices
kinematics.torso=[0,0,nan,0,0,nan];
% Variables init
datasets.selftouch=cell(1,length(datasetsNames));
for name=1:length(datasetsNames)
% Split dataset name to get names of the chains
spl=strsplit(datasetsNames{name},'_');
chain1=spl{1};
chain2=spl{2};
if contains(chain1, 'Finger') || contains(chain2, 'Finger')
finger=1;
else
finger=0;
end
% If chain1 is not being optimized, swap the chains
%if ~finger
if ~chains.(chain1)
chain1=spl{2};
chain2=spl{1};
end
%end
% Split string with word 'Arm' to get just side ...right for
% rightArm, '' for Torso
if contains(chain1, 'Arm')
name1=strsplit(chain1,'Arm');
name1=name1{1};
elseif contains(chain1, 'Finger')
name1=strsplit(chain1,'Finger');
name1=name1{1};
else
name1 = chain1;
end
if contains(chain2, 'Arm')
name2=strsplit(chain2,'Arm');
name2=name2{1};
elseif contains(chain2, 'Finger')
name2=strsplit(chain2,'Finger');
name2=name2{1};
else
name2=chain2;
end
% call prepareData to get current values
taxelStruct=prepareData(robot,datasetsNames{name},chain1,chain2,kinematics,alt,chains, optim);
%Init dataset
dataset.point = []; % Points in local frame Mx3 (Mx6 if no refPoint)
dataset.pose = []; % Mx1 int id of pose
dataset.frame = {}; % Mx1 instance of joint (chain1)
dataset.frame2 = {}; % Mx1 instance of joint (chain2)
dataset.joints = []; % Mx1 structure of joints angles (fields are 'group' names...rightArm, rightArmSkin...)
dataset.cameras = [];
dataset.refPoints = []; % Mx3(Mx6) aray of refPoints (precomputed points)
dataset.rtMat = []; % Mx1 strucure of precomputed RT matrices (fields are the same as in 'joints')
% defined in prepareData()
dataset.mins=[];
dataset.difs=[];
dataset.cops=[];
dataset.cop=[];
dataset.newTaxelsNA=[];
dataset.newTaxels=[];
dataset.name=strcat(chain1,'_',chain2);
%Load taxels in local frames
if ~ismember(chain1, {'leftFinger', 'rightFinger'})
if ~alt
chain1Original = zeros(384,6);
else
firstLocal = importdata(strcat('Dataset/Points/',chain1,'.txt'),' ',4);
chain1Original=firstLocal.data;
end
else
chain1Original = [0,0,0];
end
if ~ismember(chain2, {'leftFinger', 'rightFinger'})
if ~alt
chain2Original = zeros(384, 6);
else
firstLocal = importdata(strcat('Dataset/Points/',chain2,'.txt'),' ',4);
chain2Original=firstLocal.data;
end
else
chain2Original = [0,0,0];
end
poseID=1;
%Iterate over all 32 triangles (maximal number on one chain)
for triangleId=0:31
%Init temp variables
datasetLocal.point = [];
datasetLocal.pose = [];
datasetLocal.frame = {};
datasetLocal.frame2 = {};
datasetLocal.joints = [];
datasetLocal.cameras = [];
datasetLocal.refPoints = [];
datasetLocal.rtMat = [];
datasetLocal.mins=[];
datasetLocal.difs=[];
datasetLocal.numberOfCops=[];
datasetLocal.numberOfTaxels=[];
datasetLocal.cops=[];
datasetLocal.cop=[];
datasetLocal.newTaxelsNA=[];
datasetLocal.newTaxels=[];
%Iterate over 12 taxels on each triangle
for taxelId=1:12
index=triangleId*12+taxelId;
%check if there is field with given index and if there
%is more than X (0) values in it
if isstruct(taxelStruct.(strcat('s',num2str(index)))) && size(taxelStruct.(strcat('s',num2str(index))).secondTaxel,1)>0
% In each field of 'taxelStruct' there can be more
% records
for i=1:size(taxelStruct.(strcat('s',num2str(index))).secondTaxel,1)
% Joint angles for given activation
angles=taxelStruct.(strcat('s',num2str(index))).angles(i);
%index of second taxel (in 'chain2Original')
a=taxelStruct.(strcat('s',num2str(index))).secondTaxelId(i);
c2=chain2;
if optim.refPoints && ~all([chains.(chain1),chains.(c2)])
% Take refPoint of chain2
datasetLocal.refPoints=[datasetLocal.refPoints;taxelStruct.(strcat('s',num2str(index))).secondTaxel(i,:)];
% Assing taxel on chain1 in local frame
datasetLocal.point=[datasetLocal.point;chain1Original(index,1:3)];%.*1000
else
%Assign taxel on chain1,chain2 in local
%frames
datasetLocal.point=[datasetLocal.point;chain1Original(index,1:3),chain2Original(a,1:3)];%.*1000,.*1000
end
datasetLocal.joints=[datasetLocal.joints;angles];
%Find frame from string, e.g. rightTriangle15
if ~ismember(chain1, {'leftFinger', 'rightFinger'})
datasetLocal.frame{end+1,1}=strcat(name1,'Taxel',num2str(triangleId),'_',num2str(taxelId-1));
else
datasetLocal.frame{end+1,1}=chain1;
end
% Find frame of second taxel...integer division of (a-1)/12
% a-1 because a is in matlab indexing and
% triangles are numbered from 0
%e.g. a=12...a//12=1, but we need (a-1)//12=0
if ~ismember(chain2, {'leftFinger', 'rightFinger'})
datasetLocal.frame2{end+1,1} = strcat(name2,'Taxel',num2str(fix((a-1)/12)),'_',num2str(mod(a-1,12)));
else
datasetLocal.frame2{end+1,1} = chain2;
end
%datasetLocal.pose=[datasetLocal.pose;fix((a-1)/12)];
datasetLocal.pose=[datasetLocal.pose;poseID];
% Assing mins and difs...just for visualization
datasetLocal.mins=[datasetLocal.mins;taxelStruct.(strcat('s',num2str(index))).mins(i)];
datasetLocal.difs=[datasetLocal.difs;taxelStruct.(strcat('s',num2str(index))).difs(i,:)];
poseID=poseID+1;
% Torso is computed everytime to prevent
% error in getTF
matrices.torso=eye(4);
datasetLocal.rtMat=[datasetLocal.rtMat;matrices];
%Just for visualization
datasetLocal.cops=[datasetLocal.cops;taxelStruct.(strcat('s',num2str(index))).cops{i}];
datasetLocal.cop=[datasetLocal.cop;taxelStruct.(strcat('s',num2str(index))).cop{i}];
datasetLocal.newTaxelsNA=[datasetLocal.newTaxelsNA;taxelStruct.(strcat('s',num2str(index))).newTaxelsNA{i}];
datasetLocal.newTaxels=[datasetLocal.newTaxels;taxelStruct.(strcat('s',num2str(index))).newTaxels{i}];
end
end
end
%if there were more than X (10) activations on one
%triangle, assign them to the global dataset
if size(datasetLocal.point,1)>0
dataset.refPoints=[dataset.refPoints;datasetLocal.refPoints];
dataset.point=[dataset.point;datasetLocal.point];
dataset.joints=[dataset.joints;datasetLocal.joints];
dataset.frame=[dataset.frame;datasetLocal.frame];
dataset.frame2=[dataset.frame2;datasetLocal.frame2];
dataset.pose=[dataset.pose;datasetLocal.pose];
dataset.rtMat=[dataset.rtMat;datasetLocal.rtMat];
dataset.mins=[dataset.mins;datasetLocal.mins];
dataset.difs=[dataset.difs;datasetLocal.difs];
dataset.cops=[dataset.cops;datasetLocal.cops];
dataset.cop=[dataset.cop;datasetLocal.cop];
dataset.newTaxelsNA=[dataset.newTaxelsNA;datasetLocal.newTaxelsNA];
dataset.newTaxels=[dataset.newTaxels;datasetLocal.newTaxels];
end
end
dataset.avg=mean(dataset.mins);
dataset.avgDifs=mean(dataset.difs);
% Assign dataset into output datasets
datasets.selftouch{name}=dataset;
end
end