1 #include "mex.h"
2 #include <iostream>
3 #include <memory>
4
5 #include "ezc3d.h"
6 #include "utils.h"
7 #include "Header.h"
8 #include "Parameters.h"
9 #include "Data.h"
10
parseParam(mxDouble * data,const std::vector<size_t> & dimension,std::vector<int> & param_data,size_t idxInData=0,size_t currentIdx=0)11 size_t parseParam(
12 mxDouble* data,
13 const std::vector<size_t> &dimension,
14 std::vector<int> ¶m_data,
15 size_t idxInData=0,
16 size_t currentIdx=0) {
17 if (dimension[currentIdx] == 0)
18 return SIZE_MAX;
19
20 for (size_t i=0; i<dimension[currentIdx]; ++i){
21 if (currentIdx == dimension.size()-1){
22 param_data.push_back (static_cast<int>(data[idxInData]));
23 ++idxInData;
24 }
25 else
26 idxInData = parseParam(data, dimension, param_data, idxInData, currentIdx + 1);
27 }
28 return idxInData;
29 }
30
parseParam(mxDouble * data,const std::vector<size_t> & dimension,std::vector<float> & param_data,size_t idxInData=0,size_t currentIdx=0)31 size_t parseParam(
32 mxDouble* data,
33 const std::vector<size_t> &dimension,
34 std::vector<float> ¶m_data,
35 size_t idxInData=0,
36 size_t currentIdx=0) {
37 if (dimension[currentIdx] == 0)
38 return SIZE_MAX;
39
40 for (size_t i = 0; i<dimension[currentIdx]; ++i){
41 if (currentIdx == dimension.size()-1){
42 param_data.push_back (static_cast<float>(data[idxInData]));
43 ++idxInData;
44 }
45 else
46 idxInData = parseParam(data, dimension, param_data, idxInData, currentIdx + 1);
47 }
48 return idxInData;
49 }
parseParam(mxArray * data,const std::vector<size_t> & dimension,std::vector<std::string> & param_data,size_t idxInData=0,size_t currentIdx=0)50 size_t parseParam(
51 mxArray* data,
52 const std::vector<size_t> &dimension,
53 std::vector<std::string> ¶m_data,
54 size_t idxInData=0,
55 size_t currentIdx=0) {
56 if (dimension[currentIdx] == 0)
57 return SIZE_MAX;
58
59 for (size_t i = 0; i < dimension[currentIdx]; ++i){
60 if (currentIdx == dimension.size()-1){
61 mxArray *cell(mxGetCell(data, static_cast<mwIndex>(idxInData)));
62 param_data.push_back (toString(cell));
63 ++idxInData;
64 }
65 else
66 idxInData = parseParam(data, dimension, param_data, idxInData, currentIdx + 1);
67 }
68 return idxInData;
69 }
70
checkLongestStrParam(const std::vector<std::string> & param_data)71 size_t checkLongestStrParam(
72 const std::vector<std::string> ¶m_data) {
73 size_t longest(0);
74 for (size_t i=0; i<param_data.size(); ++i){
75 if (param_data[i].size() > longest)
76 longest = param_data[i].size();
77 }
78 return longest;
79 }
80
81
mexFunction(int nlhs,mxArray * [],int nrhs,const mxArray * prhs[])82 void mexFunction(
83 int nlhs,
84 mxArray *[],
85 int nrhs,
86 const mxArray *prhs[]) {
87 // Check inputs and outputs
88 if (nrhs != 2)
89 mexErrMsgTxt("Input argument must be valids path and c3d structure.");
90 if (mxIsChar(prhs[0]) != 1)
91 mexErrMsgTxt("Input argument 1 must be a valid path to write.");
92 if (mxIsStruct(prhs[1]) != 1)
93 mexErrMsgTxt("Input argument 2 must be a valid c3d structure.");
94 if (nlhs != 0)
95 mexErrMsgTxt("Too many output arguments.");
96
97 // Receive the path
98 std::string path(toString(prhs[0]));
99 if (path.size() == 0)
100 mexErrMsgTxt("Input argument 1 must be a valid path to write.");
101 std::string extension = ".c3d";
102 if (path.find_last_of(".") > path.size()
103 || path.substr(path.find_last_of(".")).compare(extension)){
104 path += extension;
105 }
106
107 // Receive the structure
108 const mxArray * c3dStruct(prhs[1]);
109 mxArray *header = mxGetField(c3dStruct, 0, "header");
110 if (!header)
111 mexErrMsgTxt("'header' is not accessible in the structure.");
112 mxArray *parameters = mxGetField(c3dStruct, 0, "parameters");
113 if (!parameters)
114 mexErrMsgTxt("'parameters' is not accessible in the structure.");
115 mxArray *data = mxGetField(c3dStruct, 0, "data");
116 if (!data)
117 mexErrMsgTxt("'data' is not accessible in the structure.");
118 mxArray *dataPoints = mxGetField(data, 0, "points");
119 mxArray *dataMetaPoints = mxGetField(data, 0, "meta_points");
120 mxArray *dataAnalogs = mxGetField(data, 0, "analogs");
121
122 // Setup important factors
123 if (!dataPoints)
124 mexErrMsgTxt("'data.points' is not accessible in the structure.");
125 const mwSize *dimsPoints = mxGetDimensions(dataPoints);
126 size_t nFramesPoints;
127 if (mxGetNumberOfDimensions(dataPoints) == 3)
128 nFramesPoints = dimsPoints[2];
129 else if (mxGetNumberOfDimensions(dataPoints) == 2)
130 nFramesPoints = 1;
131 else {
132 nFramesPoints = INT_MAX;
133 mexErrMsgTxt("'data.points' should be in "
134 "format XYZ x nPoints x nFrames.");
135 }
136 size_t nPointsComponents(dimsPoints[0]);
137 size_t nPoints(dimsPoints[1]);
138 if (nPointsComponents < 3 || nPointsComponents > 4)
139 mexErrMsgTxt("'data.points' should be in "
140 "format XYZ x nPoints x nFrames.");
141
142 // Check if metadat exists and if so their dimensions
143 mxArray *metadataResidual = nullptr;
144 mxArray *metadataCamMasks = nullptr;
145 if (dataMetaPoints) {
146 metadataResidual = mxGetField(dataMetaPoints, 0, "residuals");
147 if (metadataResidual){
148 if (mxGetNumberOfDimensions(metadataResidual) == 3) {
149 const mwSize *dimsResiduals = mxGetDimensions(metadataResidual);
150 if (dimsResiduals[0] * dimsResiduals[1] * dimsResiduals[2] == 0) {
151 // Act as if there is no residual
152 metadataResidual = nullptr;
153 }
154 else if(!(dimsResiduals[0] == 1 && dimsResiduals[1] == nPoints
155 && dimsResiduals[2] == nFramesPoints) ){
156 mexErrMsgTxt("'data.meta_points.residuals' should be in "
157 "format 1 x nPoints x nFrames.");
158 }
159 }
160 else {
161 mexErrMsgTxt("'data.meta_points.residuals' should be in "
162 "format 1 x nPoints x nFrames.");
163 }
164 }
165
166 metadataCamMasks = mxGetField(dataMetaPoints, 0, "camera_masks");
167 if (metadataCamMasks){
168 if (mxGetNumberOfDimensions(metadataCamMasks) == 3) {
169 const mwSize *dimsResiduals = mxGetDimensions(metadataCamMasks);
170 if (dimsResiduals[0] * dimsResiduals[1] * dimsResiduals[2] == 0) {
171 // Act as if there is no masks
172 metadataCamMasks = nullptr;
173 }
174 else if(!(dimsResiduals[0] == 7 && dimsResiduals[1] == nPoints
175 && dimsResiduals[2] == nFramesPoints) ){
176 mexErrMsgTxt("'data.meta_points.camera_masks' should be in "
177 "format 7 x nPoints x nFrames.");
178 }
179 }
180 else {
181 mexErrMsgTxt("'data.meta_points.camera_masks' should be in "
182 "format 7 x nPoints x nFrames.");
183 }
184 }
185 }
186
187 if (!dataAnalogs)
188 mexErrMsgTxt("'data.analogs' is not accessible in the structure.");
189 if (mxGetNumberOfDimensions(dataAnalogs) != 2)
190 mexErrMsgTxt("'data.analogs' should be in format nFrames x nAnalogs.");
191 const mwSize *dimsAnalogs = mxGetDimensions(dataAnalogs);
192 size_t nAnalogs(dimsAnalogs[1]);
193 size_t nFramesAnalogs(dimsAnalogs[0]);
194
195 size_t nFrames(0);
196 size_t nSubframes(0);
197 if (nFramesPoints != 0){
198 if (nFramesAnalogs % nFramesPoints != 0)
199 mexErrMsgTxt("Number of frames of Points and Analogs "
200 "should be a multiple of an integer");
201 nFrames = nFramesPoints;
202 nSubframes = nFramesAnalogs/nFramesPoints;
203 } else {
204 nFrames = nFramesAnalogs;
205 nSubframes = 1;
206 }
207
208
209
210 // Create a fresh c3d which will be fill with c3d struct
211 ezc3d::c3d c3d;
212
213 // Fill the header field that won't autoadjusts
214 size_t firstFrame(0);
215 mxArray *headerPoint = mxGetField(header, 0, "points");
216 if (headerPoint){
217 mxArray *firstFrameField = mxGetField(headerPoint, 0, "firstFrame");
218 if (firstFrameField){
219 mxDouble* firstFrameData = mxGetDoubles(firstFrameField);
220 if (firstFrameData)
221 // 1-based
222 firstFrame = static_cast<size_t>(firstFrameData[0] - 1);
223 }
224 }
225 c3d.setFirstFrame(firstFrame);
226
227 // Get the names of the points
228 mxArray *parametersPoints = mxGetField(parameters, 0, "POINT");
229 if (!parametersPoints) {
230 mexErrMsgTxt("'parameters.POINT' is not accessible in the structure.");
231 }
232
233 size_t cmpLabels(1);
234 std::string mod("");
235 std::vector<std::string> pointLabels;
236 while (true) {
237 mxArray *parametersPointsLabels = mxGetField(parametersPoints, 0, ("LABELS" + mod).c_str());
238 if (!parametersPointsLabels) {
239 if (cmpLabels == 1){
240 mexErrMsgTxt("'parameters.POINT.LABELS' parameters "
241 "is not accessible in the structure.");
242 } else {
243 break;
244 }
245 }
246 mxArray *valuePointsLabels = mxGetField(
247 parametersPointsLabels, 0, DATA_FIELD);
248 if (!valuePointsLabels) {
249 mexErrMsgTxt(("'parameters.POINT.LABELS" + mod + "." + std::string(DATA_FIELD)
250 + " parameters is not accessible in the structure.")
251 .c_str());
252 }
253
254 for (size_t i=0; i<mxGetM(valuePointsLabels) * mxGetN(valuePointsLabels); ++i) {
255 mxArray *pointLabelsPtr = mxGetCell(valuePointsLabels, i);
256 pointLabels.push_back(toString(pointLabelsPtr));
257 }
258
259 cmpLabels++;
260 mod = std::to_string(cmpLabels);
261 }
262
263 if (nPoints != pointLabels.size()) {
264 mexErrMsgTxt("'parameters.POINT.LABELS' must have "
265 "the same length as nPoints of the data.");
266 }
267 // Add them to the c3d
268 for (size_t i=0; i<pointLabels.size(); ++i)
269 c3d.point(pointLabels[i]);
270
271
272 // Get the names of the analogs
273 mxArray *groupAnalogs = mxGetField(parameters, 0, "ANALOG");
274 if (!groupAnalogs)
275 mexErrMsgTxt("'parameters.ANALOG' is not accessible in the structure.");
276
277 cmpLabels = 1;
278 mod = "";
279 std::vector<std::string> analogsLabels;
280 while (true) {
281 mxArray *groupAnalogsLabels = mxGetField(groupAnalogs, 0, ("LABELS" + mod).c_str());
282 if (!groupAnalogsLabels){
283 if (cmpLabels == 1){
284 mexErrMsgTxt("'parameters.ANALOG.LABELS' parameters "
285 "is not accessible in the structure.");
286 } else {
287 break;
288 }
289 }
290 mxArray *valueAnalogsLabels = mxGetField(groupAnalogsLabels, 0, DATA_FIELD);
291 if (!valueAnalogsLabels) {
292 mexErrMsgTxt(("'parameters.ANALOG.LABELS" + mod + "." + std::string(DATA_FIELD)
293 + " parameters is not accessible in the structure.")
294 .c_str());
295 }
296
297 for (size_t i=0; i<mxGetM(valueAnalogsLabels) * mxGetN(valueAnalogsLabels); ++i){
298 mxArray *analogsLabelsPtr = mxGetCell(valueAnalogsLabels, i);
299 analogsLabels.push_back(toString(analogsLabelsPtr));
300 }
301
302 cmpLabels++;
303 mod = std::to_string(cmpLabels);
304 }
305 if (nAnalogs != analogsLabels.size())
306 mexErrMsgTxt("'parameters.ANALOG.LABELS' must have "
307 "the same length as nAnalogs of the data.");
308 // Add them to the c3d
309 for (size_t i=0; i<analogsLabels.size(); ++i) {
310 c3d.analog(analogsLabels[i]);
311 }
312
313 // Fill the parameters
314 for (int g=0; g<mxGetNumberOfFields(parameters); ++g){ // top level
315 std::string groupName(mxGetFieldNameByNumber(parameters, g));
316 mxArray* groupField(mxGetFieldByNumber(parameters, 0, g));
317 if (!groupField) {
318 mexErrMsgTxt("Unexplained error while gathering the parameters. "
319 "Please report this");
320 }
321
322 mxArray* metadataField(mxGetField(groupField, 0, METADATA_FIELD));
323 if (metadataField) {
324 std::string description;
325 bool isLocked(false);
326 mxArray* descriptionField(
327 mxGetField(metadataField, 0, DESCRIPTION_FIELD));
328 if (descriptionField) {
329 description = toString(descriptionField);
330 }
331
332 mxArray* isLockedField(
333 mxGetField(metadataField, 0, IS_LOCKED_FIELD));
334 if (isLockedField) {
335 isLocked = toBool(isLockedField);
336 }
337 c3d.setGroupMetadata(groupName, description, isLocked);
338 }
339
340 for (int p=0; p<mxGetNumberOfFields(groupField); ++p){
341 std::string paramName(mxGetFieldNameByNumber(groupField, p));
342 if (!paramName.compare(METADATA_FIELD)) {
343 continue;
344 }
345
346 mxArray* paramField(mxGetFieldByNumber(groupField, 0, p));
347 // Copy the parameters into the c3d,
348 // but skip those who are already done
349 if ( !(!groupName.compare("POINT") && !paramName.compare("USED"))
350 && !(!groupName.compare("POINT") && !paramName.compare("FRAMES"))
351 && !(!groupName.compare("POINT") && !paramName.compare("LABELS"))
352 && !(!groupName.compare("ANALOG") && !paramName.compare("USED"))
353 && !(!groupName.compare("ANALOG") && !paramName.compare("LABELS"))
354 && !(!groupName.compare("ANALOG") && !paramName.compare("SCALE"))
355 && !(!groupName.compare("ANALOG") && !paramName.compare("OFFSET"))
356 && !(!groupName.compare("ANALOG") && !paramName.compare("UNITS"))) {
357 std::vector<size_t> dimension;
358 size_t nDim;
359 mxArray* valueField(mxGetField(paramField, 0, DATA_FIELD));
360
361 if (!valueField)
362 nDim = 0;
363 else
364 nDim =mxGetNumberOfDimensions(valueField);
365
366 if (nDim == 0)
367 dimension.push_back(0);
368 else if (nDim == 2 && mxGetDimensions(valueField)[0]
369 * mxGetDimensions(valueField)[1] == 0)
370 dimension.push_back(0);
371 else if (nDim == 2 && mxGetDimensions(valueField)[0]
372 * mxGetDimensions(valueField)[1] == 1)
373 dimension.push_back(1);
374 else
375 for (size_t i = 0; i < nDim; ++i)
376 dimension.push_back(mxGetDimensions(valueField)[i]);
377
378 // Special cases
379 if ( (!groupName.compare("POINT")
380 && !paramName.compare("DESCRIPTIONS"))
381 && dimension[0] != nPoints)
382 continue;
383 if ( (!groupName.compare("ANALOG")
384 && !paramName.compare("DESCRIPTIONS"))
385 && dimension[0] != nAnalogs)
386 continue;
387
388 ezc3d::ParametersNS::GroupNS::Parameter newParam(paramName);
389 try {
390 ezc3d::DATA_TYPE type(
391 c3d.parameters().group(groupName)
392 .parameter(paramName).type());
393
394 if (type == ezc3d::DATA_TYPE::INT
395 || type == ezc3d::DATA_TYPE::BYTE) {
396 std::vector<int> data;
397 parseParam(mxGetDoubles(valueField), dimension, data);
398 newParam.set(data, dimension);
399 } else if (type == ezc3d::DATA_TYPE::FLOAT) {
400 std::vector<float> data;
401 parseParam(mxGetDoubles(valueField), dimension, data);
402 newParam.set(
403 std::vector<double>(data.begin(), data.end()),
404 dimension);
405 } else if (type == ezc3d::DATA_TYPE::CHAR) {
406 std::vector<std::string> data;
407 parseParam(valueField, dimension, data);
408 dimension.pop_back();
409 newParam.set(data, dimension);
410 } else
411 mexErrMsgTxt(std::string(
412 "Unrecognized type for parameter."
413 + groupName + "." + paramName + ".")
414 .c_str());
415 } catch (std::invalid_argument) {
416 if (!valueField || mxIsDouble(valueField)) {
417 std::vector<float> data;
418 parseParam(mxGetDoubles(valueField), dimension, data);
419 newParam.set(
420 std::vector<double>(data.begin(), data.end()),
421 dimension);
422 } else if (mxIsCell(valueField)) {
423 std::vector<std::string> data;
424 parseParam(valueField, dimension, data);
425 newParam.set(data, dimension);
426 } else if (mxIsChar(valueField)) {
427 std::vector<std::string> data;
428 data.push_back (toString(valueField));
429 dimension.pop_back(); // Matlab inserts length already
430 newParam.set(data, dimension);
431 } else
432 mexErrMsgTxt(std::string(
433 "Unrecognized type for parameter."
434 + groupName + "." + paramName + ".")
435 .c_str());
436 }
437
438 // Get the metadata for this parameter
439 mxArray* descriptionField(
440 mxGetField(paramField, 0, DESCRIPTION_FIELD));
441 if (descriptionField) {
442 newParam.description(toString(descriptionField));
443 }
444
445 mxArray* isLockedField(
446 mxGetField(paramField, 0, IS_LOCKED_FIELD));
447 if (isLockedField) {
448 if (toBool(isLockedField)) {
449 newParam.lock();
450 }
451 else {
452 newParam.unlock();
453 }
454 }
455
456 c3d.parameter(groupName, newParam);
457 }
458 }
459 }
460
461 // Fill the data
462 mxDouble* allDataPoints = mxGetDoubles(dataPoints);
463 mxDouble* allResiduals = nullptr;
464 mxDouble* allMasks = nullptr;
465 if (dataMetaPoints){
466 if (metadataResidual){
467 allResiduals = mxGetDoubles(metadataResidual);
468 }
469 if (metadataCamMasks){
470 allMasks = mxGetDoubles(metadataCamMasks);
471 }
472 }
473 mxDouble* allDataAnalogs = mxGetDoubles(dataAnalogs);
474 for (size_t f=0; f<nFrames; ++f){
475 ezc3d::DataNS::Frame frame;
476 ezc3d::DataNS::Points3dNS::Points pts;
477 for (size_t i=0; i<nPoints; ++i){
478 ezc3d::DataNS::Points3dNS::Point pt;
479 pt.x(static_cast<float>(
480 allDataPoints[nPointsComponents*i+0+f*3*nPoints]));
481 pt.y(static_cast<float>(
482 allDataPoints[nPointsComponents*i+1+f*3*nPoints]));
483 pt.z(static_cast<float>(
484 allDataPoints[nPointsComponents*i+2+f*3*nPoints]));
485 if (allResiduals) {
486 pt.residual(static_cast<float>(allResiduals[i+f*nPoints]));
487 }
488 if (allMasks) {
489 std::vector<bool> camMasks;
490 for (size_t c = 0; c<7; ++c){
491 camMasks.push_back(
492 static_cast<bool>(allMasks[c+7*i+f*7*nPoints]));
493 }
494 pt.cameraMask(camMasks);
495 }
496 pts.point(pt);
497 }
498
499 ezc3d::DataNS::AnalogsNS::Analogs analogs;
500 for (size_t sf=0; sf<nSubframes; ++sf){
501 ezc3d::DataNS::AnalogsNS::SubFrame subframe;
502 for (size_t i=0; i<nAnalogs; ++i){
503 ezc3d::DataNS::AnalogsNS::Channel c;
504 c.data(static_cast<float>(
505 allDataAnalogs[nFramesAnalogs*i+sf+f*nSubframes]));
506 subframe.channel(c);
507 }
508 analogs.subframe(subframe);
509 }
510 frame.add(pts, analogs);
511 c3d.frame(frame);// Add the previously created frame
512 }
513 c3d.write(path);
514 return;
515 }
516