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> &param_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> &param_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> &param_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> &param_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