spelling typo
[mldemos:allopens-mldemos.git] / _AlgorithmsPlugins / SEDS / SEDS.cpp
1 #include "SEDS.h"\r
2 #ifndef M_PI\r
3 #define M_PI 3.14159265358979323846\r
4 #endif\r
5 \r
6 double NLOpt_Compute_J(unsigned nPar, const double *x, double *grad, void *f_data)\r
7 {\r
8     SEDS *seds = (SEDS *) f_data;\r
9 \r
10     Vector p(nPar),dJ(nPar);\r
11     p.Set(x,nPar);\r
12 \r
13     double J = seds->Compute_J(p, dJ);\r
14     if(grad)\r
15     {\r
16         for (int i=0; i<nPar; i++)\r
17             grad[i] = dJ[i];\r
18     }\r
19 \r
20     double J_tmp = 1e20;\r
21     if (seds->displayData.size()>0)\r
22         J_tmp = seds->displayData.back();\r
23 \r
24     J_tmp = min(J,J_tmp);\r
25     seds->displayData.push_back(J_tmp);\r
26 #ifdef USEQT\r
27     // we paint the data\r
28     seds->PaintData(seds->displayData);\r
29 #endif\r
30 \r
31     return J;\r
32 }\r
33 \r
34 void NLOpt_Constraint(unsigned nCtr, double *result, unsigned nPar, const double* x, double* grad, void* f_data)\r
35 {\r
36     SEDS *seds = (SEDS *) f_data;\r
37 \r
38     Vector c(nCtr);\r
39     Matrix dc(nCtr,nPar);\r
40     seds->Compute_Constraints(c, dc, false);\r
41 \r
42     for (int i=0; i<nCtr; i++){\r
43         result[i] = c[i];\r
44         if(grad)\r
45         {\r
46             for (int j=0; j<nPar; j++)\r
47                 grad[i*nPar+j] = dc(i,j);\r
48         }\r
49     }\r
50 }\r
51 \r
52 //constructor\r
53 SEDS::SEDS()\r
54 {\r
55     Options.max_iter = 1000;\r
56     Options.tol_stopping = 1e-4;\r
57     Options.delta = 1.0e-5;\r
58     Options.tol_mat_bias = 1e-12;\r
59     Options.perior_opt = 1;\r
60     Options.mu_opt = 1;\r
61     Options.sigma_x_opt = 1;\r
62     Options.display = 1;\r
63     Options.objective = 1; //i.e. using likelihood\r
64     Options.eps_margin = 1e-4;\r
65     Options.SEDS_Ver = 2;\r
66     d = 0;\r
67     nData = 0;\r
68 #ifdef USEQT\r
69     displayLabel = 0;\r
70 #endif\r
71 }\r
72 \r
73 /* Parsing the input commands to the solver */\r
74 bool SEDS::Parse_Input(int argc, char **argv, char** file_data, char** file_model, char** file_output)\r
75 {\r
76     if ((argc-1)%2!=0 && argc > 1 && strcmp(argv[1],"-h")){\r
77         cout << "Improper number of input arguments!" << endl;\r
78         cout << "Leaving optimization.!" << endl;\r
79         return 0;\r
80     }\r
81 \r
82     for (int i=1; i < argc; i++){\r
83         if (!strcmp(argv[i],"-dfile")) //name of the data file to be passed to the solver\r
84             *file_data = argv[i+1];\r
85         else if (!strcmp(argv[i],"-mfile"))// name of the file contains the model's initial guess to be passed to the solver\r
86             *file_model = argv[i+1];\r
87         else if (!strcmp(argv[i],"-ofile"))// name of the file contains the model's initial guess to be passed to the solver\r
88             *file_output = argv[i+1];\r
89         else if (!strcmp(argv[i],"-i"))  // an integer defining the maximum number of iterations\r
90             Options.max_iter = atoi(argv[i+1]);\r
91         else if (!strcmp(argv[i],"-t")) // a double variable defining an additive bias term on Sigmas\r
92             Options.tol_mat_bias = atof(argv[i+1]);\r
93         else if (!strcmp(argv[i],"-s")) // a double variable defining the stopping threshold\r
94             Options.tol_stopping = atof(argv[i+1]);\r
95         else if (!strcmp(argv[i],"-o")){// defining the objective function\r
96             if (!strcmp(argv[i+1],"mse"))\r
97                 Options.objective = 0;\r
98         }\r
99         else if (!strcmp(argv[i],"-op"))// a 0 or 1 integer defining whether Prior should be optimized\r
100             Options.perior_opt = atoi(argv[i+1]) > 0;\r
101         else if (!strcmp(argv[i],"-om"))// a 0 or 1 integer defining whether Mu should be optimized\r
102             Options.mu_opt = atoi(argv[i+1]) > 0;\r
103         else if (!strcmp(argv[i],"-os"))// a 0 or 1 integer defining whether Sigma should be optimized\r
104             Options.sigma_x_opt = atoi(argv[i+1]) > 0;\r
105         else if (!strcmp(argv[i],"-d"))// a 0 or 1 integer defining whether to display results on the screen\r
106             Options.display = atoi(argv[i+1]) > 0;\r
107         else if (!strcmp(argv[i],"-h")){// displaying help\r
108 \r
109             cout << "\n SEDS optimization toolbox. This function finds an optimal value of" << endl;\r
110             cout << " a Gaussian Mixture Model under the constraint of ensuring its global" << endl;\r
111             cout << " asymptotic stability." << endl;\r
112             cout << " " << endl;\r
113             cout << " Syntax " << endl;\r
114             cout << "     (Working Folder)$ ./optimization_SEDS -dfile <datafile> " << endl;\r
115             cout << "                       -mfile <modelfile>  -ofile <outputfile>" << endl;\r
116             cout << "                        [optional commands]" << endl;\r
117             cout << " " << endl;\r
118             cout << " " << endl;\r
119             cout << " Inputs -----------------------------------------------------------------" << endl;\r
120             cout << " " << endl;\r
121             cout << "   o -dfile:  Data file contains demonstration datapoints." << endl;\r
122             cout << "              The package supports both binary and text formats." << endl;\r
123             cout << "              For binary files use the file extension .bin and for" << endl;\r
124             cout << "              text files use .txt" << endl;\r
125             cout << " " << endl;\r
126             cout << "              - If the data file is binary, the structure of the file is " << endl;\r
127             cout << "                     <d (int)> <nData (int)>" << endl;\r
128             cout << "                     <Data(1,:) array of nData (double)>" << endl;\r
129             cout << "                          ..." << endl;\r
130             cout << "                     <Data(2*d,:) array of nData (double)>" << endl;\r
131             cout << " " << endl;\r
132             cout << "              - If the file is in the text format, the structure of the file is " << endl;\r
133             cout << "                Each line has 2*d elements and corresponds to each datapoint" << endl;\r
134             cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;\r
135             cout << " " << endl;\r
136             cout << " " << endl;\r
137             cout << "   o -mfile:    Model file contains an initial guess for the model." << endl;\r
138             cout << "                The package supports both binary and text format." << endl;\r
139             cout << "                For binary files use the file extension .bin and for" << endl;\r
140             cout << "                text files use .txt" << endl;\r
141             cout << " " << endl;\r
142             cout << "              - If the model file is binary, the structure of the file is " << endl;\r
143             cout << "                     <d (int)> <K (int)> <Priors array of K (double)>" << endl;\r
144             cout << "                     <Mu(1,:) array of K (double)> ... <Mu(2*d,:) array of K (double)>" << endl;\r
145             cout << "                     <Sigma(1,:,1) array of 2*d (double)> ... <Sigma(2*d,:,1) array of 2*d (double)>" << endl;\r
146             cout << "                         ..." << endl;\r
147             cout << "                     <Sigma(1,:,K) array of 2*d (double)> ... <Sigma(2*d,:,K) array of 2*d (double)>" << endl;\r
148             cout << " " << endl;\r
149             cout << "              - If the file is in the text format, the structure of the file is " << endl;\r
150             cout << "                     First Line:         d" << endl;\r
151             cout << "                     Second Line:         K" << endl;\r
152             cout << "                     Third Line:         Priors" << endl;\r
153             cout << "                     Next 2*d Lines:    Mu" << endl;\r
154             cout << "                     Next 2*d Lines:    Sigma(:,:,1)" << endl;\r
155             cout << "                         ..." << endl;\r
156             cout << "                     Next 2*d Lines:    Sigma(:,:,K)" << endl;\r
157             cout << " " << endl;\r
158             cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;\r
159             cout << " " << endl;\r
160             cout << "   o -ofile:    Name of the output file. The obtained optimal value for the GMM" << endl;\r
161             cout << "                will be saved as a text file based on the following order:" << endl;\r
162             cout << "                     First Line:         d" << endl;\r
163             cout << "                     Second Line:         K" << endl;\r
164             cout << "                     Third Line:         Priors" << endl;\r
165             cout << "                     Next 2*d Lines:    Mu" << endl;\r
166             cout << "                     Next 2*d Lines:    Sigma(:,:,1)" << endl;\r
167             cout << "                         ..." << endl;\r
168             cout << "                     Next 2*d Lines:    Sigma(:,:,K)" << endl;\r
169             cout << " " << endl;\r
170             cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;\r
171             cout << " " << endl;\r
172             cout << " " << endl;\r
173             cout << " Optional Commands-------------------------------------------------------" << endl;\r
174             cout << "       -t:    a very small positive scalar to avoid instabilities in " << endl;\r
175             cout << "              Gaussian kernel [default: 10^-15]" << endl;\r
176             cout << " " << endl;\r
177             cout << "       -s:    A small positive scalar defining the stoppping tolerance for " << endl;\r
178             cout << "              the optimization solver [default: 10^-10]" << endl;\r
179             cout << " " << endl;\r
180             cout << "       -i:    maximum number of iteration for the solver [default: i_max=1000]" << endl;\r
181             cout << " " << endl;\r
182             cout << "       -p:    Most of the time, it is preferable to transform a constrained " << endl;\r
183             cout << "              optimization problem into an unconstrained one by penalizing " << endl;\r
184             cout << "              if the constrains are violated. 'cons_penalty' should be a " << endl;\r
185             cout << "              big value in comparison to the order of magnitude of data." << endl;\r
186             cout << "              If you wish to solve the real unconstrained problem, set the" << endl;\r
187             cout << "              value of 'cons_penalty' to Inf [default: 10^4]" << endl;\r
188             cout << " " << endl;\r
189             cout << "       -o:   'likelihood': use likelihood as criterion to optimize parameters " << endl;\r
190             cout << "              of GMM 'mse': use mean square error as criterion to optimize " << endl;\r
191             cout << "              parameters of GMM " << endl;\r
192             cout << " " << endl;\r
193             cout << "       -d:    An option to control whether the algorithm displays the output" << endl;\r
194             cout << "              of each iterations [default: true]" << endl;\r
195             cout << " " << endl;\r
196             cout << "       -op:   Shall the sover optimize priors? This is an option given to the" << endl;\r
197             cout << "              user if s/he wishes not to optimize the priors [default: true]" << endl;\r
198             cout << " " << endl;\r
199             cout << "       -om:   Shall the sover optimize centers? This is an option given to the" << endl;\r
200             cout << "              user if s/he wishes not to optimize the centers Mu [default: true]" << endl;\r
201             cout << " " << endl;\r
202             cout << "       -os:   Shall the sover optimize Sigma_x? This is an option given to the " << endl;\r
203             cout << "              user if s/he wishes not to optimize the Sigma_x [default: true]" << endl;\r
204             cout << " " << endl;\r
205             cout << " " << endl;\r
206             cout << "   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;\r
207             cout << "   %%%    Copyright (c) 2010 S. Mohammad Khansari-Zadeh, LASA Lab, EPFL,   %%%" << endl;\r
208             cout << "   %%%          CH-1015 Lausanne, Switzerland, http://lasa.epfl.ch         %%%" << endl;\r
209             cout << "   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;\r
210             cout << " " << endl;\r
211             cout << "   The program is free for non-commercial academic use. Please contact the" << endl;\r
212             cout << "   author if you are interested in using the software for commercial purposes." << endl;\r
213             cout << "   The software must not be modified or distributed without prior permission" << endl;\r
214             cout << "   of the authors. Please acknowledge the authors in any academic publications" << endl;\r
215             cout << "   that have made use of this code or part of it. Please use this BibTex" << endl;\r
216             cout << "   reference:" << endl;\r
217             cout << " " << endl;\r
218             cout << "      S. M. Khansari Zadeh and A. Billard, 'Imitation learning of Globally " << endl;\r
219             cout << "      Stable Non-Linear Point-to-Point Robot Motions using Nonlinear" << endl;\r
220             cout << "      Programming', in Proceeding of the 2010 IEEE/RSJ International" << endl;\r
221             cout << "      Conference on Intelligent Robots and Systems (IROS 2010), Taipei," << endl;\r
222             cout << "      Taiwan, October 2010 " << endl;\r
223             cout << " " << endl;\r
224             cout << "    To get latest upadate of the software please visit" << endl;\r
225             cout << "                             http://lasa.epfl.ch/khansari" << endl;\r
226             cout << " " << endl;\r
227             cout << "    Please send your feedbacks or questions to:" << endl;\r
228             cout << "                             mohammad.khansari_at_epfl.ch\n\n" << endl;\r
229 \r
230             return 0;\r
231         }\r
232         else{\r
233             cout << "\nInvalid arguments '" << argv[i] <<"', please try again." << endl;\r
234             cout << "Use -h to get a list of possible commands." << endl;\r
235             cout << "Leaving optimization.!\n" << endl;\r
236             return 0;\r
237         }\r
238 \r
239         i++;\r
240     }\r
241 \r
242     if (*file_data == NULL){\r
243         cout << "\nThe name of the data file is not passed to the solver." << endl;\r
244         cout << "Use the option -dfile to specify the file name." << endl;\r
245         cout << "Leaving optimization.!\n" << endl;\r
246         return 0;\r
247     }\r
248     if (*file_model == NULL){\r
249         cout << "\nThe name of the model file is not passed to the solver." << endl;\r
250         cout << "Use the option -mfile to specify the file name." << endl;\r
251         cout << "Leaving optimization.!\n" << endl;\r
252         return 0;\r
253     }\r
254     if (*file_output == NULL){\r
255         cout << "\nThe name of the output file is not passed to the solver." << endl;\r
256         cout << "Use the option -ofile to specify the file name." << endl;\r
257         cout << "Leaving optimization.!\n" << endl;\r
258         return 0;\r
259     }\r
260 \r
261     return 1;\r
262 }\r
263 \r
264 /* Loading demonstration detapoint\r
265  * fileName: name of the file containing datapoints\r
266   * For binary files use the file extension .bin and for\r
267  * text files use .txt\r
268  *\r
269  *               - If the data file is binary, the structure of the file is\r
270  *                               <d (int)> <nData (int)> <Data(1,:) array of nData (double)> ... <Data(2*d,:) array of nData (double)>\r
271  *\r
272  *               - If the file is in the text format, the structure of the file is\r
273  *                               Each line has 2*d elements and corresponds to each datapoint\r
274  * For more detailed information see the file 'Communicate2Exe.m'\r
275 */\r
276 bool SEDS::loadData(const char fileName[], char type)\r
277 {\r
278     // Load coefficient of a GMM from a file\r
279 \r
280     if (type == 'b'){ //reading a binary file\r
281         //open the file\r
282         FILE *file = fopen(fileName, "rb");\r
283         if (!file)\r
284         {\r
285             std::cout << "Error: Could not open the file!" << std::endl;\r
286             return false;\r
287         }\r
288         //read from file, get nData etc\r
289         fread(&d, sizeof(int), 1, file);\r
290         fread(&nData, sizeof(int), 1, file);\r
291 \r
292         //read from file, get data matrix\r
293         Data.Resize(2*d,nData);\r
294         fread(Data.Array(), sizeof(REALTYPE), nData*2*d, file);\r
295         fclose(file);\r
296     }\r
297     else{\r
298         std::ifstream file(fileName);\r
299 \r
300         if(!file.is_open()){\r
301             std::cout << "Error: Could not open the file!" << std::endl;\r
302             return false;\r
303         }\r
304 \r
305         char tmp[1024];\r
306         double valTmp;\r
307         nData = 0;\r
308         d = 0;\r
309         // Get number of row\r
310         while(!file.eof()){\r
311             file.getline(tmp,1024);\r
312             nData++;\r
313             if (nData==1){\r
314                 // Get number of columns\r
315                 std::istringstream strm;\r
316                 strm.str(tmp);\r
317                 while (strm >> valTmp)\r
318                     d++;\r
319             }\r
320         }\r
321         nData = nData - 1;\r
322         d = d/2;\r
323         Data.Resize(2*d,nData); // note that Data' is saved in the text file\r
324         file.clear();\r
325         file.seekg(0); // returns to beginning of the file\r
326         for(unsigned int i=0;i<nData;i++){\r
327             file.getline(tmp,1024);\r
328             std::istringstream strm;\r
329             strm.str(tmp);\r
330             for(unsigned int j=0;j<2*d;j++){\r
331                 strm >> Data(j,i);\r
332             }\r
333         }\r
334         file.close();\r
335     }\r
336     return true;\r
337 }\r
338 \r
339 /* Loading initial guess of the model\r
340  * fileName: name of the file containing the model\r
341  * For binary files use the file extension .bin and for\r
342  * text files use .txt\r
343  *\r
344  *       - If the model file is binary, the structure of the file is\r
345  *       <d (int)> <K (int)> <Priors array of K (double)>\r
346  *               <Mu(1,:) array of K (double)> ... <Mu(2*d,:) array of K (double)>\r
347  *               <Sigma(1,:,1) array of 2*d (double)> ... <Sigma(2*d,:,1) array of 2*d (double)>\r
348  *                        ...\r
349  *               <Sigma(1,:,K) array of 2*d (double)> ... <Sigma(2*d,:,K) array of 2*d (double)>\r
350  *\r
351  *       - If the file is in the text format, the structure of the file is\r
352  *               First Line:            d\r
353  *               Second Line:           K\r
354  *               Third Line:            Priors\r
355  *               Next 2*d Lines:        Mu\r
356  *               Next 2*d Lines:        Sigma(:,:,1)\r
357  *                       ...\r
358  *               Next 2*d Lines:        Sigma(:,:,K)\r
359  *\r
360  * For more detailed information see the file 'Communicate2Exe.m'\r
361 */\r
362 bool SEDS::loadModel(const char fileName[], char type)\r
363 {\r
364     // Load coefficient of a GMM from a file\r
365 \r
366     if (type == 'b'){ //reading a binary file\r
367         //open the file\r
368         FILE *file = fopen(fileName, "rb");\r
369         if (!file)\r
370         {\r
371             std::cout << "Error: Could not open the file!" << std::endl;\r
372             return false;\r
373         }\r
374         //read from file, get d and K etc\r
375         fread(&d, sizeof(int), 1, file);\r
376         fread(&K, sizeof(int), 1, file);\r
377 \r
378         d /= 2;\r
379 \r
380         //read prior\r
381         Priors.Resize(K);\r
382         fread(Priors.Array(), sizeof(REALTYPE), K, file);\r
383 \r
384         //read Mu\r
385         Mu.Resize(2*d,K);\r
386         fread(Mu.Array(), sizeof(REALTYPE), 2*d*K, file);\r
387 \r
388         //read Sigma\r
389         Sigma = new Matrix[K];\r
390         for (int k = 0; k < K; k++)\r
391         {\r
392             Sigma[k] = Matrix(2*d,2*d);\r
393             fread(Sigma[k].Array(), sizeof(REALTYPE), 4*d*d, file);\r
394         }\r
395 \r
396         fclose(file);\r
397     }\r
398     else{\r
399         std::ifstream file(fileName);\r
400 \r
401         if(!file.is_open()){\r
402             std::cout << "Error: Could not open the file!" << std::endl;\r
403             return false;\r
404         }\r
405 \r
406         file >> d >> K;\r
407 \r
408         //        d /= 2; // correction by Manuel Muehlig\r
409 \r
410         Priors.Resize(K);\r
411         for (int k = 0; k < K; k++)\r
412             file >> Priors[k];\r
413 \r
414         Mu.Resize(2*d,K);\r
415         for (int i = 0; i < 2*d; i++)\r
416             for (int k = 0; k < K; k++) // loop order swapped, correction by Manuel Muehlig\r
417                 file >> Mu(i,k);\r
418 \r
419         Sigma = new Matrix[K];\r
420         for (int k = 0; k < K; k++)\r
421         {\r
422             Sigma[k] = Matrix(2*d,2*d);\r
423             for (int i = 0; i < 2*d; i++)\r
424                 for (int j = 0; j < 2*d; j++)\r
425                     file >> Sigma[k](i,j);\r
426         }\r
427 \r
428         endpoint.resize(2*d);\r
429         for (unsigned int i = 0; i < 2*d; i++)\r
430         {\r
431             file >> endpoint[i];\r
432         }\r
433 \r
434         file.close();\r
435     }\r
436 \r
437     return true;\r
438 }\r
439 \r
440 /* Saving the optimal obtained model from SEDS\r
441  * fileName: name of the file to save the model.\r
442  * The model will be saved in text format based on the following strcuture:\r
443  *\r
444  *                               First Line:            d\r
445  *                               Second Line:           K\r
446  *                               Third Line:            Priors\r
447  *                               Next 2*d Lines:        Mu\r
448  *                               Next 2*d Lines:        Sigma(:,:,1)\r
449  *                               ...\r
450  *                               Next 2*d Lines:        Sigma(:,:,K)\r
451  *\r
452  * For more detailed information see the file 'Communicate2Exe.m'\r
453 */\r
454 bool SEDS::saveModel(const char fileName[])\r
455 {\r
456     // Save the dataset to a file\r
457     std::ofstream file(fileName);\r
458 \r
459     if(!file){\r
460         std::cout << "Error: Could not open the file!" << std::endl;\r
461         return false;\r
462     }\r
463 \r
464     file << d << endl; //dimension\r
465     file << K << endl << endl; //number of Gaussian\r
466 \r
467     file.precision(10); //setting the precision of writing\r
468 \r
469     for (unsigned int k = 0; k < K; k++)\r
470         file << Priors(k) << " ";\r
471     file << endl << endl;\r
472 \r
473     for (unsigned int i = 0; i < 2*d; i++){\r
474         for (unsigned int k = 0; k < K; k++)\r
475             file << Mu(i,k) << " ";\r
476         file << endl;\r
477     }\r
478     file << endl;\r
479 \r
480     for (unsigned int k = 0; k < K; k++){\r
481         for (unsigned int i = 0; i < 2*d; i++){\r
482             for (unsigned int j = 0; j < 2*d; j++)\r
483                 file << Sigma[k](i,j) << " ";\r
484             file << endl;\r
485         }\r
486         file << endl;\r
487     }\r
488 \r
489     for (unsigned int i = 0; i < 2*d; i++)\r
490     {\r
491         file << endpoint[i] << " ";\r
492     }\r
493     file << endl;\r
494     file.close();\r
495 \r
496     return true;\r
497 }\r
498 \r
499 void SEDS::preprocess_sigma(){\r
500     for (int k=0; k<K; k++)\r
501     {\r
502         for (int i=0; i<d; i++)\r
503         {\r
504             for (int j=0; j<d; j++)\r
505             {\r
506                 if(i==j)\r
507                 {\r
508                     Sigma[k](i,i) = fabs(Sigma[k](i,i));\r
509                     Sigma[k](i+d,i) = -fabs(Sigma[k](i+d,i));\r
510                     Sigma[k](i,i+d) = -fabs(Sigma[k](i,i+d));\r
511                     Sigma[k](i+d,i+d) = fabs(Sigma[k](i+d,i+d));\r
512                 }\r
513                 else\r
514                 {\r
515                     Sigma[k](i,j) = 0.;\r
516                     Sigma[k](i+d,j) = 0.;\r
517                     Sigma[k](i,j+d) = 0.;\r
518                     Sigma[k](i+d,j+d) = 0.;\r
519                 }\r
520             }\r
521         }\r
522     }\r
523 }\r
524 \r
525 bool SEDS::initialize_value(){\r
526     tmpData = new Matrix[K];\r
527     Pxi = new Vector[K];\r
528     h = new Vector[K];\r
529     h_tmp = new Vector[K];\r
530     prob.Resize(nData);\r
531     Pxi_Priors.Resize(nData); //a vector representing Pxi*Priors\r
532 \r
533     A = new Matrix[K];\r
534     invSigma_x = new Matrix[K];\r
535     Sigma_xdx = new Matrix[K];\r
536     Mu_x = new Vector[K];\r
537     Mu_xd = new Vector[K];\r
538     invSigma = new Matrix[K];\r
539     B_Inv = new Matrix[d];\r
540 \r
541     detSigma.Resize(K);\r
542     detSigma_x.Resize(K);\r
543     rArs.Resize(d,d);\r
544     rBrs.Resize(d,d);\r
545     rAvrs.Resize(d); //vector form of rArs!!!\r
546     tmp_A.Resize(d,2*d); //will be used for eq to Matlab [eye(2);A(:,:,i)]\r
547     B.Resize(d,d);\r
548 \r
549     sum_dp.Resize(K);\r
550 \r
551     if (Options.objective)\r
552         tmp_mat.Resize(2*d,nData);\r
553     else\r
554         tmp_mat.Resize(d,nData);\r
555 \r
556     for(int k=0; k<K; k++) {\r
557         Pxi[k] = Vector(nData);\r
558         h[k] = Vector(nData);\r
559         h_tmp[k] = Vector(nData);\r
560         invSigma_x[k]=Matrix(d,d);\r
561         Sigma_xdx[k]=Matrix(d,d);\r
562         invSigma[k]=Matrix(2*d,2*d);\r
563         A[k]=Matrix(d,d);\r
564         Mu_x[k] = Vector(d);\r
565         Mu_xd[k] = Vector(d);\r
566     }\r
567 \r
568     if (Options.objective){ //likelihood objective function\r
569         rSrs.Resize(2*d,2*d);\r
570         L = new Matrix[K];\r
571         for(int k=0; k<K; k++){\r
572             tmpData[k] = Matrix(2*d,nData);\r
573             L[k] = Matrix(2*d,2*d);\r
574         }\r
575     }else{ //mse objective function\r
576         rSrs.Resize(d,d);\r
577         Sigma_x = new Matrix[K];\r
578         L_x = new Matrix[K];\r
579         X = Data.GetMatrix(0,d-1,0,nData-1);\r
580         Xd = Data.GetMatrix(d,2*d-1,0,nData-1);\r
581         Xd_hat.Resize(d,nData);\r
582         for(int k=0; k<K; k++){\r
583             tmpData[k] = Matrix(d,nData);\r
584             L_x[k] = Matrix(d,d);\r
585             Sigma_x[k] = Matrix(d,d);\r
586         }\r
587     }\r
588 \r
589     C_Lyapunov.Resize(d,d);\r
590     for (int i=0;i<d;i++)\r
591         C_Lyapunov(i,i) = 1;\r
592     return true;\r
593 }\r
594 \r
595 //QPixmap pm(320,240);\r
596 //QLabel lbl;\r
597 void SEDS::PaintData(std::vector<float> data)\r
598 {\r
599 #ifndef USEQT\r
600     return;\r
601 #else\r
602     if(!displayLabel) return;\r
603     int w = displayLabel->width();\r
604     int h = displayLabel->height();\r
605     QPixmap pm(w,h);\r
606     //QBitmap bitmap(w,h);\r
607     //bitmap.clear();\r
608     //pm.setMask(bitmap);\r
609     pm.fill(Qt::transparent);\r
610     QPainter painter(&pm);\r
611 \r
612     int cnt = data.size();\r
613     int pad = 4;\r
614     QPointF oldPoint;\r
615     double minVal = FLT_MAX;\r
616     double maxVal = -FLT_MAX;\r
617     for(int i=0; i< data.size(); i++)\r
618     {\r
619         if(minVal > data[i]) minVal = data[i];\r
620         if(maxVal < data[i]) maxVal = data[i];\r
621     }\r
622     if (minVal == maxVal)\r
623     {\r
624         minVal = 0;\r
625     }\r
626 \r
627     painter.setBrush(Qt::NoBrush);\r
628     painter.setPen(QPen(QColor(200,200,200), 0.5));\r
629     int steps = 3;\r
630     for(int i=0; i<=steps; i++)\r
631     {\r
632         painter.drawLine(QPoint(0, i/(float)steps*(h-2*pad) + pad), QPoint(w, i/(float)steps*(h-2*pad) + pad)); // horizontal\r
633     }\r
634     steps = steps*w/h;\r
635     for(int i=0; i<=steps; i++)\r
636     {\r
637         painter.drawLine(QPoint(i/(float)steps*w, 0), QPoint(i/(float)steps*w, h)); // vertical\r
638     }\r
639     painter.setRenderHint(QPainter::Antialiasing);\r
640 \r
641     painter.setPen(QPen(Qt::black, 1.5));\r
642     for(int i=0; i< data.size(); i++)\r
643     {\r
644         float value = data[i];\r
645         if (value != value) continue;\r
646         float x = i/(float)cnt*w;\r
647         float y = (1 - (value-minVal)/(maxVal - minVal)) * (float)(h-2*pad) + pad;\r
648         QPointF point(x, y);\r
649         if(i) painter.drawLine(oldPoint, point);\r
650         //painter.drawEllipse(point, 3, 3);\r
651         oldPoint = point;\r
652     }\r
653     painter.setPen(QPen(Qt::black, 0.5));\r
654     painter.setBrush(QColor(255,255,255,200));\r
655     painter.drawRect(QRect(190,5,100,45));\r
656     painter.setPen(QPen(Qt::black, 1));\r
657     QFont font = painter.font();\r
658     font.setPointSize(8);\r
659     painter.setFont(font);\r
660     painter.drawText(QPointF(w*2/3, 12), QString("J_0: %1").arg(data[0]));\r
661     painter.drawText(QPointF(w*2/3, 22), QString("J_F: %1").arg(data[data.size()-1]));\r
662     displayLabel->setPixmap(pm);\r
663     qApp->processEvents(QEventLoop::ExcludeUserInputEvents);\r
664 #endif\r
665 }\r
666 \r
667 \r
668 /* Running optimization solver to find the optimal values for the model.\r
669  * The result will be saved in the variable p\r
670 */\r
671 bool SEDS::Optimize(){\r
672     displayData.clear();\r
673     /*string str = "test.txt";\r
674     loadModel(str.c_str());\r
675     str = "data.txt";\r
676     loadData(str.c_str());*/\r
677     initialize_value();\r
678     preprocess_sigma();\r
679 \r
680     if (K==1)\r
681         Options.perior_opt = false;\r
682 \r
683     if (Options.objective){\r
684         nPar = Options.perior_opt*K + Options.mu_opt*K*d + Options.sigma_x_opt*K*d*(d+1) + K*d*d + (Options.SEDS_Ver-1)*d*d;\r
685         p.Resize(nPar); //a vector of parameters\r
686         GMM_2_Parameters_Likelihood(p);\r
687     }else{\r
688         nPar = Options.perior_opt*K + Options.mu_opt*K*d + Options.sigma_x_opt*K*d*(d+1)/2 + K*d*d + (Options.SEDS_Ver-1)*d*d;\r
689         p.Resize(nPar); //a vector of parameters\r
690         GMM_2_Parameters_MSE(p);\r
691     }\r
692     nCtr = K*d + (Options.SEDS_Ver-1)*d;\r
693 \r
694     //-running NLOpt--------------------------------------------------------------------------------------\r
695     //double lb[2] = { -HUGE_VAL, 0 }; // lower bounds\r
696     nlopt::opt opt(Options.optimizationType, nPar); // algorithm and dimensionality\r
697     //nlopt_set_lower_bounds(opt, lb);\r
698     opt.set_min_objective(NLOpt_Compute_J, this);\r
699 \r
700     //nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-8);\r
701     //nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-8);\r
702 \r
703     opt.set_xtol_rel(Options.tol_stopping);\r
704     opt.set_maxeval(Options.max_iter);\r
705 \r
706     std::vector<double> vec_tol(nCtr);\r
707     for (int i=0; i<nCtr; i++)\r
708         vec_tol[i] = Options.tol_stopping;\r
709 \r
710     opt.add_inequality_mconstraint(NLOpt_Constraint, this, vec_tol);\r
711 \r
712     double minf; // the minimum objective value, upon return\r
713 \r
714     std::vector<double> p_std(nPar);\r
715     for (int i=0; i<nPar; i++)\r
716         p_std[i] = p[i];\r
717 \r
718     double min0 = Compute_J(p);\r
719 \r
720     int nlopt_result = opt.optimize(p_std, minf);\r
721     if ( nlopt_result < 0) {\r
722         printf("nlopt failed!\n");\r
723     }\r
724     else {\r
725         for (int i=0; i<nPar; i++)\r
726             p[i] = p_std[i];\r
727         printf("Decrease the objective function from %0.10g to %0.10g\n", min0, minf);\r
728     }\r
729     printf("exit flag = %d\n", nlopt_result);\r
730     //--------------------------------------------------------------------------------------------\r
731 \r
732 \r
733     //forming Priors, Mu, and Sigma from parameters p\r
734     if (Options.objective)\r
735         Parameters_2_GMM_Likelihood(p);\r
736     else{\r
737         Parameters_2_GMM_MSE(p);\r
738         Priors /= Priors.Sum();\r
739         for (int k=0;k<K;k++){\r
740             for (int i=0;i<d;i++){\r
741                 for (int j=0;j<d;j++){\r
742                     Sigma[k](i,j) = Sigma_x[k](i,j);\r
743                     Sigma[k](i+d,j) = Sigma_xdx[k](i,j);\r
744                     Sigma[k](j,i+d) = Sigma_xdx[k](i,j); // corrected version from Manuel Muehlig\r
745                     //Sigma[k](j,i+d) = Sigma_xdx[k](j,i);\r
746                 }\r
747             }\r
748         }\r
749     }\r
750 \r
751     CheckConstraints(A);\r
752     return true;\r
753 }\r
754 \r
755 /* This function computes the sensitivity of Cost function w.r.t. optimization parameters.\r
756  * The result is saved in the Vector dJ. The returned value of function is J.\r
757  * Don't mess with this function. Very sensitive and a bit complicated!\r
758 */\r
759 double SEDS::Compute_J(Vector pp, Vector& dJ) //compute the objective function and derivative w.r.t parameters (updated in vector dJ)\r
760 {\r
761     double J = Compute_J(pp); //computing the cost function\r
762 \r
763     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
764     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
765     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
766 \r
767 \r
768     for(int i=0; i<d; i++)\r
769         tmp_A(i,i) = 1;\r
770 \r
771     double det_term;\r
772     double term,sum;\r
773     Vector dJ_dMu_k(d);\r
774     int ind_start,ind_max_col;\r
775 \r
776     if (Options.sigma_x_opt){\r
777         ind_start = 0;\r
778         ind_max_col = 2*d;\r
779     }else{\r
780         ind_start = d;\r
781         ind_max_col = d;\r
782     }\r
783 \r
784     dJ.Zero();\r
785     for(int k=0; k<K; k++){\r
786         //sensitivity wrt Priors\r
787         if (Options.perior_opt && Options.objective){ //likelihood\r
788             dJ[k] = -exp(-pp[k])*Priors[k]*sum_dp[k];\r
789             /*\r
790             h[k] = Pxi[k]*Priors[k]/Pxi_Priors;\r
791             dJ(k)=-exp(pp(k))/Priors[k]*((h[k]-Priors[k]).Sum());\r
792             */\r
793         }else if (!Options.objective){\r
794             sum = 0;\r
795             double tmp_dbl;\r
796             h_tmp[k].Zero();\r
797             tmp_mat = A[k]*X;\r
798             REALTYPE *p_tmp_mat = tmp_mat.Array();\r
799             REALTYPE *p_Xd_hat = Xd_hat.Array();\r
800             REALTYPE *p_Xd = Xd.Array();\r
801 \r
802             for (int i=0; i<d; i++){\r
803                 REALTYPE *p_h_tmp = h_tmp[k].Array();\r
804                 REALTYPE *p_h = h[k].Array();\r
805                 for (int j=0; j<nData; j++){\r
806                     tmp_dbl = *(p_h++) * (*(p_tmp_mat++)-*p_Xd_hat) * (*(p_Xd_hat++)-*(p_Xd++));\r
807                     *(p_h_tmp++) += tmp_dbl;\r
808                     sum += tmp_dbl;\r
809                 }\r
810             }\r
811             if (Options.perior_opt)\r
812                 dJ[k] = exp(-pp[k])*Priors[k]*sum;\r
813             /*\r
814                 h_tmp[k] = h[k]^(((A[k]*X-Xd_hat)^(Xd_hat-Xd)).SumRow()); //This vector is common in all dJ computation.\r
815                 Thus, I defined it as a variable to save some computation power\r
816                 dJ(k)= h_tmp[k].Sum();  //derivative of priors(k) w.r.t. p(k)\r
817                 */\r
818         }\r
819 \r
820         if (Options.mu_opt)\r
821         {\r
822             if (Options.objective){ //likelihood\r
823                 tmp_A.InsertSubMatrix(0,d,A[k].Transpose(),0,d,0,d); // eq to Matlab [eye(2) A(:,:,i)']\r
824                 dJ_dMu_k=-((tmp_A*invSigma[k])*tmpData[k])*h[k];\r
825                 dJ.InsertSubVector(counter_mu,dJ_dMu_k,0,d);\r
826                 counter_mu += d;\r
827             }\r
828             else{ //mse\r
829                 tmp_mat = invSigma_x[k]*tmpData[k];\r
830                 REALTYPE *p_tmp_mat = tmp_mat.Array();\r
831                 REALTYPE *p_dJ = &dJ(counter_mu);\r
832 \r
833                 for (int i=0; i<d; i++){\r
834                     REALTYPE *p_h_tmp = h_tmp[k].Array();\r
835                     for (int j=0; j<nData; j++)\r
836                         *p_dJ += *(p_tmp_mat++) * (*(p_h_tmp++));\r
837                     p_dJ++;\r
838                     counter_mu++;\r
839                 }\r
840 \r
841                 /*\r
842                 dJ_dMu_k = (tmpData[k]*invSigma_x[k]).Transpose()*h_tmp[k];\r
843                 dJ.InsertSubVector(K+k*d,dJ_dMu_k,0,d);\r
844                 */\r
845             }\r
846         }\r
847 \r
848         //sensitivity wrt sigma\r
849         if (Options.objective) //likelihood\r
850             det_term = (detSigma(k)<0) ? -1:1; //det_term=sign(det_term)\r
851         else\r
852             det_term = (detSigma_x(k)<0) ? -1:1; //det_term=sign(det_term)\r
853 \r
854         if (Options.objective){ //likelihood\r
855             for (int i=0;i<ind_max_col;i++){\r
856                 int j = (Options.sigma_x_opt) ? i:d;\r
857                 while (j<2*d){\r
858                     rSrs.Zero();\r
859                     rSrs(j,i)=1;\r
860                     rSrs = rSrs*L[k].Transpose() + L[k]*rSrs.Transpose();\r
861 \r
862                     rAvrs = (-A[k] * rSrs.GetMatrix(0,d-1,0,d-1)+ rSrs.GetMatrix(d,2*d-1,0,d-1))*invSigma_x[k] * Mu_x[k];\r
863                     tmp_mat = (invSigma[k]*(rSrs*invSigma[k]))*tmpData[k];\r
864                     double tmp_dbl = (-0.5)*det_term*(invSigma[k]*rSrs).Trace();\r
865                     Vector tmp_vec = invSigma[k].GetMatrix(0,2*d-1,d,2*d-1)*rAvrs;\r
866 \r
867                     REALTYPE *p_tmp_mat = tmp_mat.Array();\r
868                     REALTYPE *p_tmpData = tmpData[k].Array();\r
869                     sum = 0;\r
870 \r
871                     for (int i=0; i<2*d; i++){\r
872                         REALTYPE *p_h = h[k].Array();\r
873                         for (int j=0; j<nData; j++){\r
874                             sum -= (0.5 * (*p_tmp_mat++) * (*p_tmpData) +\r
875                                     (i==0)*tmp_dbl + //derivative with respect to det Sigma which is in the numenator\r
876                                     (*p_tmpData++) * tmp_vec[i]) * (*p_h++); //since Mu_xi_d = A*Mu_xi, thus we should consider its effect here\r
877                         }\r
878                     }\r
879                     dJ(counter_sigma) = sum;\r
880                     counter_sigma++;\r
881                     j++;\r
882 \r
883                     /*\r
884                     Vector tmp_vec = (((invSigma[k]*(rSrs*invSigma[k]))*tmpData[k])^tmpData[k]).SumRow();\r
885 \r
886                     dJ(K+K*d+k*d*(2*d+1)+i_c-1) = ( tmp_vec*0.5 +\r
887                                                   (-0.5)*det_term*(invSigma[k]*rSrs).Trace() + //derivative with respect to det Sigma which is in the numenator\r
888                                                   tmpData[k].Transpose()*(invSigma[k].GetMatrix(0,2*d-1,d,2*d-1)*rAvrs)) //since Mu_xi_d = A*Mu_xi, thus we should consider its effect here\r
889                                                   *h[k]*(-1);\r
890                     */\r
891                 }\r
892             }\r
893         }else{ //mse\r
894             for (int i=0;i<d;i++){\r
895                 for (int j=0;j<d;j++){\r
896                     if (Options.sigma_x_opt && j>=i){\r
897                         rSrs.Zero();\r
898                         rSrs(j,i)=1;\r
899                         rSrs = rSrs*L_x[k].Transpose() + L_x[k]*rSrs.Transpose();\r
900 \r
901                         tmp_mat = (invSigma_x[k]*rSrs*invSigma_x[k])*tmpData[k];\r
902                         double tmp_dbl = -(invSigma_x[k]*rSrs).Trace();\r
903 \r
904                         REALTYPE *p_tmp_mat = tmp_mat.Array();\r
905                         REALTYPE *p_tmpData = tmpData[k].Array();\r
906                         sum = 0;\r
907 \r
908                         for (int ii=0; ii<d; ii++){\r
909                             REALTYPE *p_h_tmp = h_tmp[k].Array();\r
910                             for (int jj=0; jj<nData; jj++){\r
911                                 sum +=  (*p_h_tmp++) * ((*p_tmp_mat++) * (*p_tmpData++) //derivative w.r.t. Sigma in exponential\r
912                                                         + (ii == 0)*tmp_dbl); //derivative with respect to det Sigma which is in the numenator\r
913 \r
914                                 //the above term (i==0) is just to sum temp_dbl once\r
915                             }\r
916                         }\r
917                         dJ(counter_sigma) = 0.5*sum;\r
918                         counter_sigma++;\r
919 \r
920                         /*\r
921                         dJ(K+K*d+k*d*(2*d+1)+i_c-1) =  0.5*(\r
922                                                         ((((invSigma_x[k]*rSrs*invSigma_x[k]*tmpData[k])^tmpData[k]).SumRow()  + //derivative w.r.t. Sigma in exponential\r
923                                                         -det_term*(invSigma_x[k]*rSrs.GetMatrix(0,d-1,0,d-1)).Trace())^h_tmp[k]/2) //derivative with respect to det Sigma which is in the numenator\r
924                                                          ).Sum();\r
925                         */\r
926                     }\r
927 \r
928                     sum = 0;\r
929                     rSrs.Zero();\r
930                     rSrs(j,i)=1;\r
931                     tmp_mat = rSrs*X;\r
932                     REALTYPE *p_tmp_mat = tmp_mat.Array();\r
933                     REALTYPE *p_Xd_hat = Xd_hat.Array();\r
934                     REALTYPE *p_Xd = Xd.Array();\r
935 \r
936                     for (int ii=0; ii<d; ii++){\r
937                         REALTYPE *p_h = h[k].Array();\r
938                         for (int jj=0; jj<nData; jj++){\r
939                             sum += *(p_h++) * (*(p_tmp_mat++)) * (*(p_Xd_hat++) - *(p_Xd++));\r
940                         }\r
941                     }\r
942                     dJ(counter_A) = sum;\r
943                     counter_A++;\r
944                     // dJ(counter_A) = sum(sum((rSrs*x).*dJdxd).*h(:,k)');  %derivative of A\r
945 \r
946                 }\r
947             }\r
948         }\r
949     }\r
950 \r
951     dJ /= nData;\r
952     return J;\r
953 }\r
954 \r
955 /* This function computes the sensitivity of Cost function w.r.t. optimization parameters.\r
956  * The result is saved in the Vector dJ. The returned value of function is J.\r
957  * Don't mess with this function. Very sensitive and a bit complicated!\r
958 */\r
959 double SEDS::Compute_J(Vector pp){\r
960 \r
961     double J = 0;\r
962     if (Options.objective){\r
963         Parameters_2_GMM_Likelihood(pp);\r
964     }else{\r
965         Parameters_2_GMM_MSE(pp);\r
966     }\r
967 \r
968     //computing likelihood:\r
969     Pxi_Priors.Zero();\r
970 \r
971 \r
972     for (int k=0; k<K; k++){\r
973         int d_tmp;\r
974         double tmp_den;\r
975         REALTYPE *p_X;\r
976 \r
977         if (Options.objective){ //likelihod\r
978             tmp_den = sqrt(pow(2*M_PI,2*d)*fabs(detSigma[k])+DBL_MIN);\r
979             d_tmp = 2*d;\r
980             p_X = Data.Array();\r
981         }\r
982         else{ //mse\r
983             tmp_den = sqrt(pow(2*M_PI,d)*fabs(detSigma_x[k])+DBL_MIN);\r
984             d_tmp = d;\r
985             p_X = X.Array();\r
986         }\r
987 \r
988         REALTYPE *p_tmpData = tmpData[k].Array();\r
989         for(int j=0; j<d_tmp; j++){\r
990             double tmp_dbl = Mu(j,k);\r
991             for(int i=0; i<nData; i++)\r
992                 *p_tmpData++ = (*p_X++) - tmp_dbl;\r
993         }\r
994 \r
995         if (Options.objective){ //likelihod\r
996             tmp_mat = invSigma[k]*tmpData[k];\r
997         }\r
998         else{ //mse\r
999             tmp_mat = invSigma_x[k]*tmpData[k];\r
1000         }\r
1001 \r
1002         REALTYPE *p_tmp_mat = tmp_mat.Array();\r
1003         REALTYPE *p_Pxi = Pxi[k].Array();\r
1004         REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
1005         p_tmpData = tmpData[k].Array();\r
1006         prob.Zero();\r
1007 \r
1008         for(int i=0; i<d_tmp; i++){\r
1009             REALTYPE *p_prob = prob.Array();\r
1010             for(int j=0; j<nData; j++){\r
1011                 if (i<d_tmp-1){\r
1012                     *p_prob++ += (*p_tmp_mat++) * (*p_tmpData++);\r
1013                 }\r
1014                 else{\r
1015                     *p_prob += (*p_tmp_mat++) * (*p_tmpData++);\r
1016                     *p_Pxi = exp(-0.5*(*p_prob++))/tmp_den;\r
1017                     *(p_Pxi_Priors++) += (*p_Pxi++)*Priors[k];\r
1018                 }\r
1019             }\r
1020         }\r
1021         /*\r
1022         for(int j=0; j<d; j++)\r
1023             tmpData[k].SetRow(Mu(j,k),j);\r
1024 \r
1025         tmpData[k]=X-tmpData[k]; // remove centers from data\r
1026 \r
1027         prob = ((invSigma_x[k]*tmpData[k])^tmpData[k]).SumRow();//cf the expontential in gaussian pdf\r
1028 \r
1029         double tmp_den = sqrt(pow(2*M_PI,d)*fabs(detSigma_x[k])+DBL_MIN);\r
1030 \r
1031         for(int j=0; j<nData; j++){\r
1032             Pxi[k][j] = exp(-0.5*prob[j])/tmp_den;\r
1033             Pxi_Priors[j] += Pxi[k][j]*Priors[k];\r
1034         }\r
1035         */\r
1036     }\r
1037 \r
1038     //computing GMR\r
1039     if (Options.objective){ //likelihood\r
1040         for (int k=0; k<K; k++){\r
1041             REALTYPE *p_h = h[k].Array();\r
1042             REALTYPE *p_Pxi = Pxi[k].Array();\r
1043             REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
1044             sum_dp[k] = 0;\r
1045 \r
1046             for (int j=0; j<nData; j++){\r
1047                 *p_h = (*p_Pxi++)/(*p_Pxi_Priors++)*Priors[k];\r
1048                 sum_dp[k] += (*p_h++) - Priors[k];\r
1049             }\r
1050         }\r
1051     }else{\r
1052         for (int k=0; k<K; k++){\r
1053             tmp_mat = A[k]*X;\r
1054             REALTYPE *p_tmp_mat = tmp_mat.Array();\r
1055             REALTYPE *p_Xd_hat = Xd_hat.Array();\r
1056             REALTYPE *p_Pxi = Pxi[k].Array();\r
1057             REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
1058 \r
1059             for(int i=0; i<d; i++)\r
1060             {\r
1061                 REALTYPE *p_h = h[k].Array();\r
1062 \r
1063                 for(int j=0; j<nData; j++){\r
1064                     if (i==0)\r
1065                         *p_h = *(p_Pxi++)/(*(p_Pxi_Priors++)) * Priors[k];\r
1066                     if (k==0)\r
1067                         *(p_Xd_hat++) = *(p_h++) * (*p_tmp_mat++);\r
1068                     else\r
1069                         *(p_Xd_hat++) += *(p_h++) * (*p_tmp_mat++);\r
1070                 }\r
1071             }\r
1072 \r
1073             /*\r
1074             h[k] = Pxi[k]*Priors[k]/Pxi_Priors;\r
1075             if (k==0)\r
1076                 Xd_hat = Vector_Multiply(tmp_mul,h[k])^(A[k]*X);\r
1077             else\r
1078                 Xd_hat += Vector_Multiply(tmp_mul,h[k])^(A[k]*X);\r
1079             */\r
1080         }\r
1081     }\r
1082 \r
1083     //calc J\r
1084     if (Options.objective) {//likelihood\r
1085         REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
1086         for(int i=0; i<nData ; i++)\r
1087             J -= log(*p_Pxi_Priors++);\r
1088     }\r
1089     else{\r
1090         REALTYPE *p_Xd_hat = Xd_hat.Array();\r
1091         REALTYPE *p_Xd = Xd.Array();\r
1092         for(int i=0; i<d ; i++)\r
1093             for(int j=0; j<nData ; j++)\r
1094                 J += 0.5 * ((*p_Xd_hat) - (*p_Xd)) * ((*p_Xd_hat++) - (*p_Xd++));\r
1095         //J = 0.5*((Xd_hat-Xd)^(Xd_hat-Xd)).Sum();\r
1096     }\r
1097     J /= nData;\r
1098     return J;\r
1099 }\r
1100 \r
1101 /* Computing the stability constraints and their derivatives */\r
1102 void SEDS::Compute_Constraints(Vector &c, Matrix &dc, bool used_for_penalty){\r
1103     int i1_tmp;\r
1104     double tmp_detB, term;\r
1105     int counter_sigma = Options.perior_opt*K + Options.mu_opt*K*d; //the index at which sigma should start\r
1106     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
1107     int counter_C = counter_A + K*d*d; //the index at which C_Lyapunov should start\r
1108     if (Options.objective) //likelihood\r
1109         counter_C += Options.sigma_x_opt*K*d*(d+1)/2; //because likelihood has more variable than mse\r
1110 \r
1111     c.Zero();\r
1112     dc.Zero();\r
1113 \r
1114     int ind_start,ind_max_col;\r
1115     if (Options.sigma_x_opt){\r
1116         ind_start = 0;\r
1117         ind_max_col = 2*d;\r
1118     }else{\r
1119         ind_start = d;\r
1120         ind_max_col = d;\r
1121     }\r
1122 \r
1123     if (Options.constraintCriterion){ //Principal Minor\r
1124         //constraints\r
1125         for (int k=0; k<K; k++)//for all states\r
1126         {\r
1127             i1_tmp = 1;\r
1128             B = C_Lyapunov*A[k]+A[k].Transpose()*C_Lyapunov; //define B\r
1129 \r
1130             //computing the constraints (this part is basicly an exact rewrite of the matlab version)\r
1131             for (int i=0; i<d; i++) //for all dimensions\r
1132             {\r
1133                 B_Inv[i]=B.GetMatrix(0,i,0,i).Inverse(&tmp_detB);//get inverse and determinants of minors\r
1134 \r
1135                 if (!used_for_penalty || i1_tmp*tmp_detB+Options.eps_margin > 0)\r
1136                     c(k*d+i) = i1_tmp*tmp_detB + Options.eps_margin;\r
1137 \r
1138                 //computing the sensitivity of the constraints to the parameters\r
1139                 if (Options.objective){ //likelihood\r
1140                     int i_c = Options.sigma_x_opt*k*d*(d+1) + k*d*d;\r
1141                     for (int ii=0;ii<ind_max_col;ii++){\r
1142                         int jj = (Options.sigma_x_opt) ? ii:d;\r
1143                         while (jj<2*d){\r
1144                             rSrs.Zero();\r
1145                             rSrs(jj,ii) = 1;\r
1146                             rSrs = rSrs*L[k].Transpose() + L[k]*rSrs.Transpose();\r
1147                             rArs = (-A[k] * rSrs.GetMatrix(0,d-1,0,d-1) + rSrs.GetMatrix(d,2*d-1,0,d-1)) * invSigma_x[k];\r
1148                             rBrs = C_Lyapunov*rArs + rArs.Transpose()*C_Lyapunov;\r
1149 \r
1150                             if (i==0)\r
1151                                 term = rBrs(0,0);\r
1152                             else\r
1153                                 term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1154 \r
1155                             dc(k*d+i, counter_sigma + i_c) = i1_tmp*term;\r
1156 \r
1157                             if (Options.SEDS_Ver == 2 && jj>=d && ii<d){\r
1158                                 //derivative with respect to the Lyapunov stuffs\r
1159                                 rArs.Zero(); //in fact it is rCrc, but to avoid defining an extra variable, I used rArs\r
1160                                 rArs(ii,jj-d) = 1;\r
1161                                 rBrs = rArs*A[k]+A[k].Transpose()*rArs;\r
1162 \r
1163                                 if (i==0)\r
1164                                     term = rBrs(0,0);\r
1165                                 else\r
1166                                     term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1167 \r
1168                                 dc(k*d+i,counter_C + ii*d+(jj-d)) = i1_tmp*term;\r
1169                             }\r
1170 \r
1171                             jj++;\r
1172                             i_c++;\r
1173                         }\r
1174                     }\r
1175                 }else{ //mse\r
1176                     for (int ii=0; ii<=i; ii++){\r
1177                         for (int jj=0; jj<=i; jj++){\r
1178                             rArs.Zero();\r
1179                             rArs(jj,ii) = 1;\r
1180                             rBrs = C_Lyapunov*rArs + rArs.Transpose()*C_Lyapunov;\r
1181 \r
1182                             if (i==0)\r
1183                                 term = rBrs(0,0);\r
1184                             else\r
1185                                 term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1186 \r
1187                             dc(k*d+i,counter_A + k*d*d + ii*d + jj) = i1_tmp*term;\r
1188 \r
1189                             if (Options.SEDS_Ver == 2){\r
1190                                 //derivative with respect to the Lyapunov stuffs\r
1191                                 rArs.Zero(); //in fact it is rCrc, but to avoid defining an extra variable, I used rArs\r
1192                                 rArs(ii,jj) = 1;\r
1193                                 rBrs = rArs*A[k]+A[k].Transpose()*rArs;\r
1194 \r
1195                                 if (i==0)\r
1196                                     term = rBrs(0,0);\r
1197                                 else\r
1198                                     term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1199 \r
1200                                 dc(k*d+i,counter_C + ii*d+jj) = i1_tmp*term;\r
1201                             }\r
1202                         }\r
1203                     }\r
1204                 }\r
1205                 i1_tmp *= -1; //used to alternate sign over iterations\r
1206             }\r
1207         }\r
1208     }else{ //eigenvalue\r
1209         Vector eigVal(d), eigVal_new(d);\r
1210         Matrix M_tmp(d,d),eigVec(d,d);\r
1211 \r
1212         for (int k=0; k<K; k++)//for all states\r
1213         {\r
1214             B = C_Lyapunov*A[k]+A[k].Transpose()*C_Lyapunov; //define B\r
1215             B.EigenValuesDecomposition(eigVal,eigVec,100);\r
1216             eigVal.Sort();\r
1217             for (int i=0; i<d;i++){\r
1218                 if (!used_for_penalty || eigVal[i] + Options.eps_margin>0)\r
1219                     c(k*d + i) = eigVal[i] + Options.eps_margin;\r
1220             }\r
1221 \r
1222             if (Options.objective){ //likelihood\r
1223                 int i_c = Options.sigma_x_opt*k*d*(d+1) + k*d*d;\r
1224                 for (int ii=0;ii<ind_max_col;ii++){\r
1225                     int jj = (Options.sigma_x_opt) ? ii:d;\r
1226                     while (jj<2*d){\r
1227                         rSrs = L[k];\r
1228                         rSrs(jj,ii) += Options.delta;\r
1229                         rSrs = rSrs*(rSrs.Transpose());\r
1230                         M_tmp = rSrs.GetMatrix(d,2*d-1,0,d-1)*rSrs.GetMatrix(0,d-1,0,d-1).Inverse();\r
1231                         B = C_Lyapunov*M_tmp + M_tmp.Transpose()*C_Lyapunov;\r
1232                         B.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1233                         eigVal_new.Sort();\r
1234                         eigVal_new -= eigVal;\r
1235                         eigVal_new /= Options.delta;\r
1236 \r
1237                         for (int i=0; i<d;i++)\r
1238                             dc(k*d+i,counter_sigma + i_c) = eigVal_new[i];\r
1239 \r
1240 \r
1241                         if (Options.SEDS_Ver == 2 && jj>=d && ii<d){\r
1242                             //derivative with respect to the Lyapunov stuffs\r
1243                             M_tmp = C_Lyapunov;\r
1244                             M_tmp(ii,jj-d) += Options.delta;\r
1245                             M_tmp = M_tmp*A[k]+A[k].Transpose()*M_tmp;\r
1246                             M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1247                             eigVal_new.Sort();\r
1248                             eigVal_new -= eigVal;\r
1249                             eigVal_new /= Options.delta;\r
1250 \r
1251                             for (int i=0; i<d;i++)\r
1252                                 dc(k*d+i,counter_C + ii*d+(jj-d)) = eigVal_new[i];\r
1253                         }\r
1254 \r
1255                         jj++;\r
1256                         i_c++;\r
1257                     }\r
1258                 }\r
1259             }else{ //mse\r
1260                 for (int ii=0;ii<d;ii++){\r
1261                     for (int jj=0;jj<d;jj++){\r
1262                         M_tmp = A[k];\r
1263                         M_tmp(jj,ii) += Options.delta;\r
1264                         M_tmp = C_Lyapunov*M_tmp+M_tmp.Transpose()*C_Lyapunov;\r
1265                         M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1266                         eigVal_new.Sort();\r
1267                         eigVal_new -= eigVal;\r
1268                         eigVal_new /= Options.delta;\r
1269 \r
1270                         for (int i=0; i<d;i++)\r
1271                             dc(k*d+i,counter_A) = eigVal_new[i];\r
1272 \r
1273                         counter_A++;\r
1274 \r
1275                         if (Options.SEDS_Ver == 2){\r
1276                             //derivative with respect to the Lyapunov stuffs\r
1277                             M_tmp = C_Lyapunov;\r
1278                             M_tmp(ii,jj) += Options.delta;\r
1279                             M_tmp = M_tmp*A[k]+A[k].Transpose()*M_tmp;\r
1280                             M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1281                             eigVal_new.Sort();\r
1282                             eigVal_new -= eigVal;\r
1283                             eigVal_new /= Options.delta;\r
1284 \r
1285                             for (int i=0; i<d;i++)\r
1286                                 dc(k*d+i,counter_C + ii*d+jj) = eigVal_new[i];\r
1287                         }\r
1288                     }\r
1289                 }\r
1290             }\r
1291         }\r
1292     }\r
1293 \r
1294 \r
1295     //constraint on positive definiteness of C_Lyapunov\r
1296     if (Options.SEDS_Ver == 2){\r
1297         Vector eigVal(d), eigVal_new(d);\r
1298         Matrix M_tmp(d,d),eigVec(d,d);\r
1299 \r
1300         C_Lyapunov.EigenValuesDecomposition(eigVal,eigVec,100);\r
1301         eigVal.Sort();\r
1302         for (int i=0; i<d;i++){\r
1303             if (!used_for_penalty || eigVal[i] + Options.eps_margin>0)\r
1304                 c(K*d + i) = -eigVal[i] + Options.eps_margin;\r
1305         }\r
1306 \r
1307         //derivative of the constraint on positive definiteness of C_Lyapunov\r
1308         for (int ii=0;ii<d;ii++){\r
1309             for (int jj=0;jj<d;jj++){\r
1310                 M_tmp = C_Lyapunov;\r
1311                 M_tmp(jj,ii) += Options.delta;\r
1312                 M_tmp(ii,jj) += Options.delta;\r
1313                 M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1314                 eigVal_new.Sort();\r
1315                 eigVal_new -= eigVal;\r
1316                 eigVal_new /= Options.delta;\r
1317 \r
1318                 for (int i=0; i<d;i++)\r
1319                     dc(K*d+i,counter_C) = -eigVal_new[i];\r
1320 \r
1321                 counter_C++;\r
1322             }\r
1323         }\r
1324     }\r
1325 }\r
1326 \r
1327 /* Computing the stability constraints */\r
1328 void SEDS::Compute_Constraints(Vector &c){\r
1329     int i1_tmp;\r
1330     double tmp_detB;\r
1331     Vector eigVal(d);\r
1332     Matrix eigVec(d,d);\r
1333     c.Zero();\r
1334 \r
1335     //constraints\r
1336     for (int k=0; k<K; k++)//for all states\r
1337     {\r
1338         i1_tmp = 1;\r
1339         B = A[k]+A[k].Transpose(); //define B\r
1340 \r
1341         if (Options.constraintCriterion){ //principal minor\r
1342             //computing the constraints (this part is basicly an exact rewrite of the matlab version)\r
1343             for (int i=0; i<d; i++) //for all dimensions\r
1344             {\r
1345                 B_Inv[i]=B.GetMatrix(0,i,0,i).Inverse(&tmp_detB);//get inverse and determinants of minors\r
1346                 c(k*d+i) = i1_tmp*tmp_detB+pow(Options.tol_mat_bias,(double)(i+1)/d);\r
1347                 i1_tmp *= -1; //used to alternate sign over iterations\r
1348             }\r
1349         }else{ //eigenvalue\r
1350             B.EigenValuesDecomposition(eigVal,eigVec,100);\r
1351             eigVal.Sort();\r
1352             c.SetSubVector(k*d,eigVal);\r
1353         }\r
1354     }\r
1355 }\r
1356 \r
1357 /* Transforming the GMM model into the vector of optimization's parameters.*/\r
1358 bool SEDS::GMM_2_Parameters_Likelihood(Vector &p){ //this is used to transform GMM to the optimization parameters\r
1359     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1360     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1361     int counter_C = counter_sigma + K*d*d + Options.sigma_x_opt*K*d*(d+1);\r
1362 \r
1363     for (int k=0; k<K; k++){\r
1364         if (Options.perior_opt)\r
1365             p[k] = -log(1.0/Priors[k]-1.0); //insert Priors to p\r
1366 \r
1367         if (Options.mu_opt){\r
1368             for(int j=0; j<d; j++) //for all dimensions\r
1369                 p(counter_mu+k*d+j)=Mu(j,k); //insert Mu\r
1370         }else{\r
1371             Mu_x[k] = Mu.GetColumn(k).GetSubVector(0,d);\r
1372         }\r
1373 \r
1374         Sigma[k].Cholesky(L[k]);//set L\r
1375 \r
1376         if (Options.sigma_x_opt){\r
1377             for(int j=0; j<2*d; j++){\r
1378                 p.InsertSubVector(counter_sigma,L[k].GetColumn(j),j,2*d-j);\r
1379                 counter_sigma += 2*d-j;\r
1380             }\r
1381         }else{\r
1382             for(int j=0; j<d; j++){\r
1383                 p.InsertSubVector(counter_sigma,L[k].GetColumn(j),d,d);\r
1384                 counter_sigma += d;\r
1385             }\r
1386         }\r
1387     }\r
1388 \r
1389     if (Options.SEDS_Ver == 2){\r
1390         Vector tmp;\r
1391         tmp.Set(C_Lyapunov.Array(),d*d);\r
1392         tmp /= 2;\r
1393         p.SetSubVector(counter_C,tmp);\r
1394     }\r
1395     return true;\r
1396 }\r
1397 \r
1398 /* Transforming the vector of optimization's parameters into a GMM model.*/\r
1399 bool SEDS::Parameters_2_GMM_Likelihood(Vector pp){ //this is used to unpack the parameters in p after optimization, to reconstruct the\r
1400     //GMM-parameters in their ususal form: Priors, Mu and Sigma.\r
1401 \r
1402     double sum=0;\r
1403     Vector col(2*d); // a temporary vector needed below\r
1404 \r
1405     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1406     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1407     int counter_C = counter_sigma + K*d*d + Options.sigma_x_opt*K*d*(d+1);\r
1408 \r
1409     for (int k=0; k<K; k=k++){\r
1410         //constructing Priors\r
1411         if (Options.perior_opt){\r
1412             Priors[k] = 1.0/(1.0+exp(-pp[k])); //extract the Priors from correspondng position in optimization vector\r
1413             sum += Priors[k];\r
1414         }\r
1415 \r
1416         //reconstructing Sigma\r
1417         if (Options.sigma_x_opt){\r
1418             for(int j=0; j<2*d; j++){ //for all dimensions\r
1419                 col.Zero();\r
1420                 for(int i=j; i<2*d; i++){\r
1421                     col(i)=pp(counter_sigma);\r
1422                     counter_sigma++;\r
1423                 }\r
1424                 L[k].SetColumn(col, j);\r
1425             }\r
1426         }else{\r
1427             for(int j=0; j<d; j++){ //for all dimensions\r
1428                 for(int i=0; i<d; i++){\r
1429                     L[k](i+d,j) = pp(counter_sigma);\r
1430                     counter_sigma++;\r
1431                 }\r
1432             }\r
1433         }\r
1434 \r
1435         Sigma[k]=L[k]*(L[k].Transpose());\r
1436 \r
1437         for(int i=0; i<2*d;i++)\r
1438             Sigma[k](i,i)+=Options.tol_mat_bias;\r
1439 \r
1440         invSigma[k]=Sigma[k].Inverse(&detSigma(k));\r
1441         invSigma_x[k]=Sigma[k].GetMatrix(0,d-1,0,d-1).Inverse(&detSigma_x[k]);\r
1442         Sigma_xdx[k]=Sigma[k].GetMatrix(d,2*d-1,0,d-1);\r
1443         A[k]=Sigma_xdx[k]*invSigma_x[k]; //dynamical system matrix.\r
1444 \r
1445         if (Options.mu_opt){ //reconstructing Mu\r
1446             Mu_x[k] = pp.GetSubVector(K+k*d,d);\r
1447         }\r
1448         Mu_xd[k] = A[k]*Mu_x[k]; //proagate the centers through the dynamical system\r
1449         for (int i=0; i<d; i++)\r
1450         {\r
1451             if (Options.mu_opt){\r
1452                 Mu(i,k) = Mu_x[k](i);\r
1453             }\r
1454             Mu(i+d,k) = Mu_xd[k](i);\r
1455         }\r
1456     }\r
1457     if (Options.perior_opt)\r
1458         Priors /= sum; //normalizing Priors\r
1459 \r
1460     if (Options.SEDS_Ver == 2){\r
1461         C_Lyapunov.Set(pp.GetSubVector(counter_C,d*d).Array(),d,d);\r
1462         C_Lyapunov = C_Lyapunov + C_Lyapunov.Transpose();\r
1463     }\r
1464 \r
1465     return true;\r
1466 }\r
1467 \r
1468 /* Transforming the GMM model into the vector of optimization's parameters.*/\r
1469 bool SEDS::GMM_2_Parameters_MSE(Vector &p){ //this is used to transform GMM to the optimization parameters\r
1470 \r
1471     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1472     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1473     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
1474     int counter_C = counter_A + K*d*d;\r
1475 \r
1476 \r
1477     for (int k=0; k<K; k++){\r
1478         if (Options.perior_opt)\r
1479             p[k] = -log(1.0/Priors[k]-1.0); //insert Priors to p\r
1480 \r
1481         if (Options.mu_opt){\r
1482             for(int j=0; j<d; j++) //for all dimensions\r
1483                 p(counter_mu+k*d+j)=Mu(j,k); //insert Mu\r
1484         }else{\r
1485             Mu_x[k] = Mu.GetColumn(k).GetSubVector(0,d);\r
1486         }\r
1487 \r
1488         Sigma_x[k] = Sigma[k].GetMatrix(0,d-1,0,d-1);\r
1489         invSigma_x[k]=Sigma_x[k].Inverse(&detSigma_x[k]);\r
1490         Sigma_xdx[k] = Sigma[k].GetMatrix(d,2*d-1,0,d-1);\r
1491         A[k] = Sigma_xdx[k]*invSigma_x[k];\r
1492         Sigma_x[k].Cholesky(L_x[k]);//set L_x\r
1493 \r
1494         for(int j=0; j<d; j++){\r
1495             if (Options.sigma_x_opt){\r
1496                 p.InsertSubVector(counter_sigma,L_x[k].GetColumn(j),j,d-j);\r
1497                 counter_sigma += d-j;\r
1498             }\r
1499 \r
1500             p.SetSubVector(counter_A,A[k].GetColumn(j));\r
1501             counter_A += d;\r
1502         }\r
1503     }\r
1504 \r
1505     if (Options.SEDS_Ver == 2){\r
1506         Vector tmp;\r
1507         tmp.Set(C_Lyapunov.Array(),d*d);\r
1508         tmp /= 2;\r
1509         p.SetSubVector(counter_C,tmp);\r
1510     }\r
1511 \r
1512     return true;\r
1513 }\r
1514 \r
1515 /* Transforming the vector of optimization's parameters into a GMM model.*/\r
1516 bool SEDS::Parameters_2_GMM_MSE(Vector pp){ //this is used to unpack the parameters in p after optimization, to reconstruct the\r
1517     //GMM-parameters in their ususal form: Priors, Mu and Sigma.\r
1518 \r
1519     Vector col(d); // a temporary vector needed below\r
1520     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1521     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1522     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
1523     int counter_C = counter_A + K*d*d;\r
1524 \r
1525     for (int k=0; k<K; k=k++){\r
1526         //constructing Priors\r
1527         if (Options.perior_opt)\r
1528             Priors[k] = 1.0/(1.0+exp(-pp[k]));\r
1529 \r
1530         //reconstructing Sigma\r
1531         for(int j=0; j<d; j++){ //for all dimensions\r
1532             col.Zero();\r
1533             for(int i=0; i<d; i++){\r
1534                 if (i>=j && Options.sigma_x_opt){\r
1535                     col(i)=pp(counter_sigma);\r
1536                     counter_sigma++;\r
1537                 }\r
1538                 A[k](i,j) = pp(counter_A);\r
1539                 counter_A++;\r
1540             }\r
1541             if (Options.sigma_x_opt)\r
1542                 L_x[k].SetColumn(col, j);\r
1543         }\r
1544 \r
1545         if (Options.sigma_x_opt){\r
1546             Sigma_x[k]=L_x[k]*(L_x[k].Transpose());\r
1547             for(int i=0; i<d;i++)\r
1548                 Sigma_x[k](i,i)+=Options.tol_mat_bias;\r
1549             invSigma_x[k]=Sigma_x[k].Inverse(&detSigma_x[k]);\r
1550         }\r
1551 \r
1552         Sigma_xdx[k] = A[k]*Sigma_x[k];\r
1553 \r
1554         if (Options.mu_opt)\r
1555             Mu_x[k] = pp.GetSubVector(counter_mu+k*d,d); //reconstructing Mu\r
1556 \r
1557         Mu_xd[k] = A[k]*Mu_x[k]; //proagate the centers through the dynamical system\r
1558         for (int i=0; i<d; i++)\r
1559         {\r
1560             if (Options.mu_opt)\r
1561                 Mu(i,k) = Mu_x[k](i);\r
1562             Mu(i+d,k) = Mu_xd[k](i);\r
1563         }\r
1564     }\r
1565 \r
1566     if (Options.SEDS_Ver == 2){\r
1567         C_Lyapunov.Set(pp.GetSubVector(counter_C,d*d).Array(),d,d);\r
1568         C_Lyapunov = C_Lyapunov + C_Lyapunov.Transpose();\r
1569     }\r
1570     return true;\r
1571 }\r
1572 \r
1573 /* checking if every thing goes well. Sometimes if the parameter\r
1574  * 'Options.cons_penalty' is not big enough, the constrains may be violated.\r
1575  * Then this function notifies the user to increase 'Options.cons_penalty'.\r
1576 */\r
1577 bool SEDS::CheckConstraints(Matrix * A){\r
1578     Vector eigvals;\r
1579     Matrix eigvects;\r
1580     Matrix B(d,d);\r
1581     c.Zero();\r
1582     int nCtrViolated = 0;\r
1583     QString str("");\r
1584 \r
1585     for(int k=0; k<K; k++)\r
1586     {\r
1587         B = C_Lyapunov*A[k]+A[k].Transpose()*C_Lyapunov;\r
1588         B.EigenValuesDecomposition(eigvals,eigvects,100);\r
1589         for(int i=0; i<d; i++){\r
1590             if(eigvals(i) > 0){\r
1591                 if (nCtrViolated == 0)\r
1592                 {\r
1593                     cout << endl;\r
1594                     cout<<"Optimization did not finish successfully. Some constraints were violated."<<endl;\r
1595                     cout<<"The error may be due to change of hard constraints to soft constrints."<<endl;\r
1596                     cout<<"To handle this error, increase the value of 'cons_penalty' and re-run the"<<endl;\r
1597                     cout<<"optimization. Output error for debugging purpose:"<<endl<<endl;\r
1598                     str.sprintf("%s","Optimization did not finish successfully. Some constraints were violated. "\r
1599                                 "The error may be due to change of hard constraints to soft constrints. "\r
1600                                 "To handle this error, increase the value of 'Constraint Penalty' and re-run the "\r
1601                                 "optimization.\nOutput error for debugging purpose:");\r
1602                 }\r
1603                 cout<< "k = " << k << "  ;  err = " << eigvals(i) << endl;\r
1604                 nCtrViolated++;\r
1605                 str.sprintf("%s %.3f",str.toStdString().c_str(),eigvals(i));\r
1606             }\r
1607         }\r
1608     }\r
1609 \r
1610     C_Lyapunov.Print();\r
1611     C_Lyapunov.EigenValuesDecomposition(eigvals,eigvects,100);\r
1612     for(int i=0; i<d; i++){\r
1613         if(eigvals(i) < 0){\r
1614             if (nCtrViolated == 0)\r
1615             {\r
1616                 cout << endl;\r
1617                 cout<<"Optimization did not finish successfully. Some constraints were violated."<<endl;\r
1618                 cout<<"The error may be due to change of hard constraints to soft constrints."<<endl;\r
1619                 cout<<"To handle this error, increase the value of 'cons_penalty' and re-run the"<<endl;\r
1620                 cout<<"optimization. Output error for debugging purpose:"<<endl<<endl;\r
1621                 str.sprintf("%s","Optimization did not finish successfully. Some constraints were violated. "\r
1622                             "The error may be due to change of hard constraints to soft constrints. "\r
1623                             "To handle this error, increase the value of 'Constraint Penalty' and re-run the "\r
1624                             "optimization.\nOutput error for debugging purpose:");\r
1625             }\r
1626             cout<< "C_Lyapunov ;  err = " << eigvals(i) << endl;\r
1627             nCtrViolated++;\r
1628             str.sprintf("%s %.3f",str.toStdString().c_str(),eigvals(i));\r
1629         }\r
1630     }\r
1631 \r
1632     if (nCtrViolated == 0)\r
1633         cout<<"Optimization finished successfully!"<<endl;\r
1634     else{\r
1635         QMessageBox *qmes = new QMessageBox ();\r
1636         qmes->addButton("Ok",QMessageBox::AcceptRole);\r
1637         qmes->setWindowTitle("Error!");\r
1638         str.sprintf("%s. In total %d constraints were violated.",str.toStdString().c_str(),nCtrViolated);\r
1639         qmes->setText(str);\r
1640         qmes->show();\r
1641     }\r
1642 \r
1643     return true;\r
1644 }\r