v0.3.5:
[mldemos:mldemos.git] / _AlgorithmsPlugins / SEDS / SEDS.cpp
1 #include "SEDS.h"
2 #ifndef M_PI
3 #define M_PI 3.14159265358979323846
4 #endif
5
6 #include <QtGui>
7
8 QPixmap pm(320,240);
9 QLabel lbl;
10
11 //constructor
12 SEDS::SEDS()
13 {
14         Options.max_iter = 10000;
15         Options.tol_stopping = 1e-10;
16         Options.tol_mat_bias = 1e-14;
17         Options.cons_penalty = 1e4;
18         Options.perior_opt = 1;
19         Options.mu_opt = 1;
20         Options.sigma_x_opt = 1;
21         Options.display = 1;
22         Options.objective = 1; //i.e. using likelihood
23         d = 0;
24         nData = 0;
25 }
26
27
28 /* Parsing the input commands to the solver */
29 bool SEDS::Parse_Input(int argc, char **argv, char** file_data, char** file_model, char** file_output)
30 {
31         if ((argc-1)%2!=0 && argc > 1 && strcmp(argv[1],"-h")){
32                 cout << "Improper number of input arguments!" << endl;
33                 cout << "Leaving optimization.!" << endl;
34                 return 0;
35         }
36
37         for (int i=1; i < argc; i++){
38                 if (!strcmp(argv[i],"-dfile")) //name of the data file to be passed to the solver
39                         *file_data = argv[i+1];
40                 else if (!strcmp(argv[i],"-mfile"))// name of the file contains the model's initial guess to be passed to the solver
41                         *file_model = argv[i+1];
42                 else if (!strcmp(argv[i],"-ofile"))// name of the file contains the model's initial guess to be passed to the solver
43                         *file_output = argv[i+1];
44                 else if (!strcmp(argv[i],"-i"))  // an integer defining the maximum number of iterations
45                         Options.max_iter = atoi(argv[i+1]);
46                 else if (!strcmp(argv[i],"-p")) // a double variable defining the penalty value
47                         Options.cons_penalty = atof(argv[i+1]);
48                 else if (!strcmp(argv[i],"-t")) // a double variable defining an additive bias term on Sigmas
49                         Options.tol_mat_bias = atof(argv[i+1]);
50                 else if (!strcmp(argv[i],"-s")) // a double variable defining the stopping threshold
51                         Options.tol_stopping = atof(argv[i+1]);
52                 else if (!strcmp(argv[i],"-o")){// defining the objective function
53                         if (!strcmp(argv[i+1],"mse"))
54                                 Options.objective = 0;
55                 }
56                 else if (!strcmp(argv[i],"-op"))// a 0 or 1 integer defining whether Prior should be optimized
57                         Options.perior_opt = atoi(argv[i+1]) > 0;
58                 else if (!strcmp(argv[i],"-om"))// a 0 or 1 integer defining whether Mu should be optimized
59                         Options.mu_opt = atoi(argv[i+1]) > 0;
60                 else if (!strcmp(argv[i],"-os"))// a 0 or 1 integer defining whether Sigma should be optimized
61                         Options.sigma_x_opt = atoi(argv[i+1]) > 0;
62                 else if (!strcmp(argv[i],"-d"))// a 0 or 1 integer defining whether to display results on the screen
63                         Options.display = atoi(argv[i+1]) > 0;
64                 else if (!strcmp(argv[i],"-h")){// displaying help
65
66                          cout << "\n SEDS optimization toolbox. This function finds an optimal value of" << endl;
67                          cout << " a Gaussian Mixture Model under the constraint of ensuring its global" << endl;
68                          cout << " asymptotic stability." << endl;
69                          cout << " " << endl;
70              cout << " Syntax " << endl;
71              cout << "     (Working Folder)$ ./optimization_SEDS -dfile <datafile> " << endl;
72              cout << "                       -mfile <modelfile>  -ofile <outputfile>" << endl;
73              cout << "                        [optional commands]" << endl;
74              cout << " " << endl;
75              cout << " " << endl;
76              cout << " Inputs -----------------------------------------------------------------" << endl;
77              cout << " " << endl;
78              cout << "   o -dfile:  Data file contains demonstration datapoints." << endl;
79              cout << "              The package supports both binary and text formats." << endl;
80              cout << "              For binary files use the file extension .bin and for" << endl;
81              cout << "              text files use .txt" << endl;
82              cout << " " << endl;
83              cout << "              - If the data file is binary, the structure of the file is " << endl;
84              cout << "                     <d (int)> <nData (int)>" << endl;
85              cout << "                     <Data(1,:) array of nData (double)>" << endl;
86              cout << "                          ..." << endl;
87              cout << "                     <Data(2*d,:) array of nData (double)>" << endl;
88              cout << " " << endl;
89              cout << "              - If the file is in the text format, the structure of the file is " << endl;
90              cout << "                Each line has 2*d elements and corresponds to each datapoint" << endl;
91              cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;
92              cout << " " << endl;
93              cout << " " << endl;
94              cout << "   o -mfile:    Model file contains an initial guess for the model." << endl;
95              cout << "                The package supports both binary and text format." << endl;
96              cout << "                For binary files use the file extension .bin and for" << endl;
97              cout << "                text files use .txt" << endl;
98              cout << " " << endl;
99              cout << "              - If the model file is binary, the structure of the file is " << endl;
100              cout << "                     <d (int)> <K (int)> <Priors array of K (double)>" << endl;
101              cout << "                     <Mu(1,:) array of K (double)> ... <Mu(2*d,:) array of K (double)>" << endl;
102              cout << "                     <Sigma(1,:,1) array of 2*d (double)> ... <Sigma(2*d,:,1) array of 2*d (double)>" << endl;
103              cout << "                         ..." << endl;
104              cout << "                     <Sigma(1,:,K) array of 2*d (double)> ... <Sigma(2*d,:,K) array of 2*d (double)>" << endl;
105              cout << " " << endl;
106              cout << "              - If the file is in the text format, the structure of the file is " << endl;
107              cout << "                     First Line:         d" << endl;
108              cout << "                     Second Line:         K" << endl;
109              cout << "                     Third Line:         Priors" << endl;
110              cout << "                     Next 2*d Lines:    Mu" << endl;
111              cout << "                     Next 2*d Lines:    Sigma(:,:,1)" << endl;
112              cout << "                         ..." << endl;
113              cout << "                     Next 2*d Lines:    Sigma(:,:,K)" << endl;
114              cout << " " << endl;
115              cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;
116              cout << " " << endl;
117              cout << "   o -ofile:    Name of the output file. The obtained optimal value for the GMM" << endl;
118              cout << "                will be saved as a text file based on the following order:" << endl;
119              cout << "                     First Line:         d" << endl;
120              cout << "                     Second Line:         K" << endl;
121              cout << "                     Third Line:         Priors" << endl;
122              cout << "                     Next 2*d Lines:    Mu" << endl;
123              cout << "                     Next 2*d Lines:    Sigma(:,:,1)" << endl;
124              cout << "                         ..." << endl;
125              cout << "                     Next 2*d Lines:    Sigma(:,:,K)" << endl;
126              cout << " " << endl;
127              cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;
128              cout << " " << endl;
129              cout << " " << endl;
130              cout << " Optional Commands-------------------------------------------------------" << endl;
131              cout << "       -t:    a very small positive scalar to avoid instabilities in " << endl;
132              cout << "              Gaussian kernel [default: 10^-15]" << endl;
133              cout << " " << endl;
134              cout << "       -s:    A small positive scalar defining the stoppping tolerance for " << endl;
135              cout << "              the optimization solver [default: 10^-10]" << endl;
136              cout << " " << endl;
137              cout << "       -i:    maximum number of iteration for the solver [default: i_max=1000]" << endl;
138              cout << " " << endl;
139              cout << "       -p:    Most of the time, it is preferable to transform a constrained " << endl;
140              cout << "              optimization problem into an unconstrained one by penalizing " << endl;
141              cout << "              if the constrains are violated. 'cons_penalty' should be a " << endl;
142              cout << "              big value in comparison to the order of magnitude of data." << endl;
143              cout << "              If you wish to solve the real unconstrained problem, set the" << endl;
144              cout << "              value of 'cons_penalty' to Inf [default: 10^4]" << endl;
145              cout << " " << endl;
146              cout << "       -o:   'likelihood': use likelihood as criterion to optimize parameters " << endl;
147              cout << "              of GMM 'mse': use mean square error as criterion to optimize " << endl;
148              cout << "              parameters of GMM " << endl;
149              cout << " " << endl;
150              cout << "       -d:    An option to control whether the algorithm displays the output" << endl;
151              cout << "              of each iterations [default: true]" << endl;
152              cout << " " << endl;
153              cout << "       -op:   Shall the sover optimize priors? This is an option given to the" << endl;
154              cout << "              user if s/he wishes not to optimize the priors [default: true]" << endl;
155              cout << " " << endl;
156              cout << "       -om:   Shall the sover optimize centers? This is an option given to the" << endl;
157              cout << "              user if s/he wishes not to optimize the centers Mu [default: true]" << endl;
158              cout << " " << endl;
159              cout << "       -os:   Shall the sover optimize Sigma_x? This is an option given to the " << endl;
160              cout << "              user if s/he wishes not to optimize the Sigma_x [default: true]" << endl;
161              cout << " " << endl;
162              cout << " " << endl;
163              cout << "   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;
164              cout << "   %%%    Copyright (c) 2010 S. Mohammad Khansari-Zadeh, LASA Lab, EPFL,   %%%" << endl;
165              cout << "   %%%          CH-1015 Lausanne, Switzerland, http://lasa.epfl.ch         %%%" << endl;
166              cout << "   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;
167              cout << " " << endl;
168              cout << "   The program is free for non-commercial academic use. Please contact the" << endl;
169              cout << "   author if you are interested in using the software for commercial purposes." << endl;
170              cout << "   The software must not be modified or distributed without prior permission" << endl;
171              cout << "   of the authors. Please acknowledge the authors in any academic publications" << endl;
172              cout << "   that have made use of this code or part of it. Please use this BibTex" << endl;
173              cout << "   reference:" << endl;
174              cout << " " << endl;
175              cout << "      S. M. Khansari Zadeh and A. Billard, 'Imitation learning of Globally " << endl;
176              cout << "      Stable Non-Linear Point-to-Point Robot Motions using Nonlinear" << endl;
177              cout << "      Programming', in Proceeding of the 2010 IEEE/RSJ International" << endl;
178              cout << "      Conference on Intelligent Robots and Systems (IROS 2010), Taipei," << endl;
179              cout << "      Taiwan, October 2010 " << endl;
180              cout << " " << endl;
181              cout << "    To get latest upadate of the software please visit" << endl;
182              cout << "                             http://lasa.epfl.ch/khansari" << endl;
183              cout << " " << endl;
184              cout << "    Please send your feedbacks or questions to:" << endl;
185              cout << "                             mohammad.khansari_at_epfl.ch\n\n" << endl;
186             
187                         return 0;
188                 }
189                 else{
190                         cout << "\nInvalid arguments '" << argv[i] <<"', please try again." << endl;
191                         cout << "Use -h to get a list of possible commands." << endl;
192                         cout << "Leaving optimization.!\n" << endl;
193                         return 0;
194                 }
195
196                 i++;
197         }
198         
199         if (*file_data == NULL){
200                 cout << "\nThe name of the data file is not passed to the solver." << endl;
201                 cout << "Use the option -dfile to specify the file name." << endl;
202                 cout << "Leaving optimization.!\n" << endl;
203                 return 0;
204         }
205         if (*file_model == NULL){
206                 cout << "\nThe name of the model file is not passed to the solver." << endl;
207                 cout << "Use the option -mfile to specify the file name." << endl;
208                 cout << "Leaving optimization.!\n" << endl;
209                 return 0;
210         }
211         if (*file_output == NULL){
212                 cout << "\nThe name of the output file is not passed to the solver." << endl;
213                 cout << "Use the option -ofile to specify the file name." << endl;
214                 cout << "Leaving optimization.!\n" << endl;
215                 return 0;
216         }
217         
218         return 1;
219 }
220
221
222 /* Loading demonstration detapoint
223  * fileName: name of the file containing datapoints
224   * For binary files use the file extension .bin and for
225  * text files use .txt
226  * 
227  *               - If the data file is binary, the structure of the file is 
228  *                               <d (int)> <nData (int)> <Data(1,:) array of nData (double)> ... <Data(2*d,:) array of nData (double)>
229  * 
230  *               - If the file is in the text format, the structure of the file is 
231  *                               Each line has 2*d elements and corresponds to each datapoint
232  * For more detailed information see the file 'Communicate2Exe.m'
233 */
234 bool SEDS::loadData(const char fileName[], char type)
235 {
236         // Load coefficient of a GMM from a file
237         
238         if (type == 'b'){ //reading a binary file
239                 //open the file
240                 FILE *file = fopen(fileName, "rb");
241                 if (!file)
242                 {
243                         std::cout << "Error: Could not open the file!" << "\n";
244                         return false;
245                 }
246                 //read from file, get nData etc
247                 fread(&d, sizeof(int), 1, file);
248                 fread(&nData, sizeof(int), 1, file);
249                 
250                 //read from file, get data matrix
251                 Data.Resize(2*d,nData);
252                 fread(Data.Array(), sizeof(REALTYPE), nData*2*d, file);
253                 fclose(file);
254         }
255         else{
256                 std::ifstream file(fileName);
257         
258                 if(!file.is_open()){
259                         std::cout << "Error: Could not open the file!" << "\n";
260                         return false;
261                 }
262
263                 char tmp[1024];
264                 double valTmp;
265                 // Get number of row
266                 while(!file.eof()){
267                         file.getline(tmp,1024);
268                         nData++;
269                         if (nData==1){
270                                 // Get number of columns
271                                 std::istringstream strm;
272                                 strm.str(tmp); 
273                                 while (strm >> valTmp)
274                                         d++;
275                         }
276                 }
277                 nData = nData - 1;
278                 d = d/2;
279                 Data.Resize(2*d,nData); // note that Data' is saved in the text file
280                 file.clear();
281                 file.seekg(0); // returns to beginning of the file
282                 for(unsigned int i=0;i<nData;i++){ 
283                         file.getline(tmp,1024);
284                         std::istringstream strm;
285                         strm.str(tmp); 
286                         for(unsigned int j=0;j<2*d;j++){
287                                 strm >> Data(j,i);
288                         }
289                 }
290                 file.close();
291         }
292         return true;
293 }
294
295
296 /* Loading initial guess of the model
297  * fileName: name of the file containing the model
298  * For binary files use the file extension .bin and for
299  * text files use .txt
300  * 
301  *       - If the model file is binary, the structure of the file is 
302  *       <d (int)> <K (int)> <Priors array of K (double)>
303  *               <Mu(1,:) array of K (double)> ... <Mu(2*d,:) array of K (double)>
304  *               <Sigma(1,:,1) array of 2*d (double)> ... <Sigma(2*d,:,1) array of 2*d (double)>
305  *                        ...
306  *               <Sigma(1,:,K) array of 2*d (double)> ... <Sigma(2*d,:,K) array of 2*d (double)>
307  * 
308  *       - If the file is in the text format, the structure of the file is 
309  *               First Line:            d
310  *               Second Line:           K
311  *               Third Line:            Priors
312  *               Next 2*d Lines:        Mu
313  *               Next 2*d Lines:        Sigma(:,:,1)
314  *                       ...
315  *               Next 2*d Lines:        Sigma(:,:,K)
316  * 
317  * For more detailed information see the file 'Communicate2Exe.m'
318 */
319 bool SEDS::loadModel(const char fileName[], char type)
320 {
321         // Load coefficient of a GMM from a file
322         
323         if (type == 'b'){ //reading a binary file
324                 //open the file
325                 FILE *file = fopen(fileName, "rb");
326                 if (!file)
327                 {
328                         std::cout << "Error: Could not open the file!" << "\n";
329                         return false;
330                 }
331                 //read from file, get d and K etc
332                 fread(&d, sizeof(int), 1, file);
333                 fread(&K, sizeof(int), 1, file);
334                 
335                 //read prior
336                 Priors.Resize(K);
337                 fread(Priors.Array(), sizeof(REALTYPE), K, file);
338                 
339                 //read Mu
340                 Mu.Resize(2*d,K);
341                 fread(Mu.Array(), sizeof(REALTYPE), 2*d*K, file);
342                 
343                 //read Sigma
344                 Sigma = new Matrix[K];
345                 for (int k = 0; k < K; k++)
346                 {
347                         Sigma[k] = Matrix(2*d,2*d);
348                         fread(Sigma[k].Array(), sizeof(REALTYPE), 4*d*d, file);
349                 }
350
351                 fclose(file);
352         }
353         else{
354                 std::ifstream file(fileName);
355         
356                 if(!file.is_open()){
357                         std::cout << "Error: Could not open the file!" << "\n";
358                         return false;
359                 }
360                 
361                 file >> d >> K;
362
363                 Priors.Resize(K);
364                 for (int k = 0; k < K; k++)
365                         file >> Priors[k];
366                 
367                 Mu.Resize(2*d,K);
368                 for (int i = 0; i < 2*d; i++)
369                         for (int k = 0; k < K; k++)
370                                 file >> Mu(i,k);
371                 
372                 Sigma = new Matrix[K];
373                 for (int k = 0; k < K; k++)
374                 {
375                         Sigma[k] = Matrix(2*d,2*d);
376                         for (int i = 0; i < 2*d; i++)
377                                 for (int j = 0; j < 2*d; j++)
378                                         file >> Sigma[k](i,j);
379                 }
380                 file.close();
381         }
382         
383         return true;
384 }
385
386
387 /* Saving the optimal obtained model from SEDS
388  * fileName: name of the file to save the model.
389  * The model will be saved in text format based on the following strcuture:
390  * 
391  *                               First Line:            d
392  *                               Second Line:           K
393  *                               Third Line:            Priors
394  *                               Next 2*d Lines:        Mu
395  *                               Next 2*d Lines:        Sigma(:,:,1)
396  *                               ...
397  *                               Next 2*d Lines:        Sigma(:,:,K)
398  * 
399  * For more detailed information see the file 'Communicate2Exe.m'
400 */
401 bool SEDS::saveModel(const char fileName[])
402 {
403         // Save the dataset to a file
404         std::ofstream file(fileName);
405         
406         if(!file){
407                 std::cout << "Error: Could not open the file!" << "\n";
408                 return false;
409         }
410         
411         file << d << endl; //dimension
412         file << K << endl << endl; //number of Gaussian
413         
414         file.precision(10); //setting the precision of writing
415         
416         for (unsigned int k = 0; k < K; k++)
417                 file << Priors(k) << " ";
418         file << endl << endl;
419         
420         for (unsigned int i = 0; i < 2*d; i++){
421                 for (unsigned int k = 0; k < K; k++)
422                         file << Mu(i,k) << " ";
423                 file << endl;
424         }
425         file << endl;
426         
427         for (unsigned int k = 0; k < K; k++){
428                 for (unsigned int i = 0; i < 2*d; i++){
429                         for (unsigned int j = 0; j < 2*d; j++)
430                                 file << Sigma[k](i,j) << " ";
431                         file << endl;
432                 }
433                 file << endl;
434         }
435         file.close();
436         
437         return true;
438 }
439
440 void SEDS::preprocess_sigma(){
441         for (int k=0; k<K; k++)
442         {
443                 for (int i=0; i<d; i++)
444                 {
445                         for (int j=0; j<d; j++)
446                         {
447                                 if(i==j)
448                                 {
449
450                                         Sigma[k](i,i) = fabs(Sigma[k](i,i));
451                                         Sigma[k](i+d,i) = -fabs(Sigma[k](i+d,i));
452                                         Sigma[k](i,i+d) = -fabs(Sigma[k](i,i+d));
453                                         Sigma[k](i+d,i+d) = fabs(Sigma[k](i+d,i+d));
454                                 }
455                                 else
456                                 {
457                                         Sigma[k](i,j) = 0.;
458                                         Sigma[k](i+d,j) = 0.;
459                                         Sigma[k](i,j+d) = 0.;
460                                         Sigma[k](i+d,j+d) = 0.;
461                                 }
462                         }
463                 }
464         }
465 }
466
467 bool SEDS::initialize_value(){
468         tmpData = new Matrix[K];
469         Pxi = new Vector[K];
470         h = new Vector[K];
471         h_tmp = new Vector[K];
472         prob.Resize(nData);
473         Pxi_Priors.Resize(nData); //a vector representing Pxi*Priors
474         
475         A = new Matrix[K];
476         L = new Matrix[K];
477         invSigma_x = new Matrix[K];
478         Sigma_xdx = new Matrix[K];
479         Mu_x = new Vector[K];
480         Mu_xd = new Vector[K];
481         invSigma = new Matrix[K];
482         B_Inv = new Matrix[d];
483         
484         detSigma.Resize(K);
485         detSigma_x.Resize(K);
486         rSrs.Resize(2*d,2*d);
487         rArs.Resize(d,d);
488         rBrs.Resize(d,d);
489         rAvrs.Resize(d); //vector form of rArs!!!
490         tmp_A.Resize(d,2*d); //will be used for eq to Matlab [eye(2);A(:,:,i)]
491         B.Resize(d,d);
492         dc.Resize(d*K,K+K*d+K*d*(2*d+1));
493         c.Resize(K*d); //a vector representing the constraints
494         if (Options.objective)
495                 tmp_mat.Resize(2*d,nData);
496         else
497                 tmp_mat.Resize(d,nData);
498                 
499         for(int k=0; k<K; k++) {
500                 Pxi[k] = Vector(nData);
501                 h[k] = Vector(nData);
502                 h_tmp[k] = Vector(nData);
503                 L[k] = Matrix(2*d,2*d);
504                 invSigma_x[k]=Matrix(d,d);
505                 Sigma_xdx[k]=Matrix(d,d);
506                 invSigma[k]=Matrix(2*d,2*d);
507                 A[k]=Matrix(d,d);
508                 Mu_x[k] = Vector(d);
509                 Mu_xd[k] = Vector(d);
510         }
511         
512         if (!Options.objective){ //mse objective function
513                 X = Data.GetMatrix(0,d-1,0,nData-1);
514                 Xd = Data.GetMatrix(d,2*d-1,0,nData-1);
515                 Xd_hat.Resize(d,nData);
516                 for(int k=0; k<K; k++)
517                         tmpData[k] = Matrix(d,nData);
518                 
519         }
520         else
521         {
522                 for(int k=0; k<K; k++)
523                         tmpData[k] = Matrix(2*d,nData);
524         }
525         return true;
526 }
527
528
529 void PaintData(std::vector<float> data)
530 {
531         QPainter painter(&pm);
532         painter.fillRect(pm.rect(), Qt::white);
533
534         int w = pm.width();
535         int h = pm.height();
536         int cnt = data.size();
537         int pad = 10;
538         QPointF oldPoint;
539         double minVal = FLT_MAX;
540         double maxVal = -FLT_MAX;
541         for(int i=0; i< data.size(); i++)
542         {
543                 if(minVal > data[i]) minVal = data[i];
544                 if(maxVal < data[i]) maxVal = data[i];
545         }
546         if (minVal == maxVal)
547         {
548                 minVal = 0;
549         }
550
551         painter.setBrush(Qt::NoBrush);
552         painter.setPen(QPen(QColor(200,200,200), 0.5));
553         int steps = 10;
554         for(int i=0; i<=steps; i++)
555         {
556                 painter.drawLine(QPoint(0, i/(float)steps*(h-2*pad) + pad), QPoint(w, i/(float)steps*(h-2*pad) + pad));
557                 painter.drawLine(QPoint(i/(float)steps*w, 0), QPoint(i/(float)steps*w, h));
558         }
559         painter.setRenderHint(QPainter::Antialiasing);
560
561         painter.setPen(QPen(Qt::black, 1.5));
562         for(int i=0; i< data.size(); i++)
563         {
564                 float value = data[i];
565                 float x = i/(float)cnt*w;
566                 float y = (1 - (value-minVal)/(maxVal - minVal)) * (float)(h-2*pad) + pad;
567                 QPointF point(x, y);
568                 if(i) painter.drawLine(oldPoint, point);
569                 //painter.drawEllipse(point, 3, 3);
570                 oldPoint = point;
571         }
572         painter.setPen(QPen(Qt::black, 0.5));
573         painter.setBrush(QColor(255,255,255,200));
574         painter.drawRect(QRect(190,5,100,45));
575         painter.setPen(QPen(Qt::black, 1));
576         painter.drawText(QPointF(200, 20), QString("J_0: %1").arg(data[0]));
577         painter.drawText(QPointF(200, 40), QString("J_F: %1").arg(data[data.size()-1]));
578         lbl.setPixmap(pm);
579         lbl.show();
580 }
581
582 std::vector<float> displayData;
583
584 /* Running optimization solver to find the optimal values for the model.
585  * The result will be saved in the variable p
586 */
587 bool SEDS::Optimize(){
588         displayData.clear();
589
590         initialize_value();
591         preprocess_sigma();
592         cout << "sigmas preprocessed!\n";
593         for(int k=0; k<K; k++)
594         {
595                 cout << "k: " << k << "\n";
596                 Sigma[k].Print();
597         }
598         p = Vector(K+K*d+K*d*(2*d+1)); //a vector of parameters
599         int counter = K*d+K;//the index at which sigma should start
600
601         for (int k=0; k<K; k++){
602                 p(k)=log(Priors(k)); //insert Priors to p
603                 
604                 for(int j=0; j<d; j++) //for all dimensions
605                         p(K+k*d+j)=Mu(j,k); //insert Mu
606                 
607                 L[k] = Sigma[k].SCholesky();//set L
608                 
609                 for(int j=0; j<2*d; j++){
610                         p.InsertSubVector(counter,L[k].GetColumn(j),j,2*d-j);
611                         counter += 2*d-j;
612                 }
613         }
614         //now p contains Priors, Mu_x and Sigma. ready to optimize
615    
616         Matrix B(K*(1+2*d*(d+1)),K*(1+2*d*(d+1)));
617         
618         //computing the mean of the data 
619         Vector mean_Data(2*d);
620         mean_Data = Data.SumColumn()/nData;
621         
622         //computing the variance of Data, will be used for initialization of B
623         Vector var_Data(2*d);
624         for (int i=0 ; i<2*d ; i++)
625                 var_Data(i) = ((Data.GetRow(i)-mean_Data(i))^(Data.GetRow(i)-mean_Data(i))).Sum(); //sum the square
626         
627         var_Data /= (nData-1); //finally normalize to get the N-1-normalized variance (as in matlab)
628         
629         double meanVar=var_Data.Sum()/(2*d); //take mean of varaince over columns
630         
631         B=B.Identity()/meanVar; //this causes problems. 
632         
633         char* str[2];
634         str[0] = (char*)"MSE";
635         str[1] = (char*)"Likelihood";
636         if (Options.display)
637         {
638                 std::cout << "\n" << "\n";
639                 std::cout << "%-------------------------------------------------------------------------------------%" << "\n";
640                 std::cout << "%          Stable Estimator of Dynamical Systems (SEDS) Optimization Solver           %" << "\n";
641                 std::cout << "%       Copyright(c) 2010 S. Mohammad Khansari Zadeh, LASA Lab, EPFL, Lausanne        %" << "\n";
642                 std::cout << "%                    Switzerland, http://lasa.epfl.ch/khansari                        %" << "\n";
643                 std::cout << "%-------------------------------------------------------------------------------------%" << "\n";
644                 std::cout << "\n\nOptimization Algorithm starts ..." << endl;
645                 printf("Using %s as the objective function ...\n\n",str[Options.objective]);
646                 
647         }
648
649         double c1=0.0001;
650         double c2=0.9;
651         int j;
652         double alpha=0;
653         Vector dp(p.Size());
654         Vector p_tmp(p.Size());
655         Vector dJ(p.Size());
656         Vector dJ_new(p.Size());
657
658         double J=Compute_J(dJ,p); //compute dJ and J
659         double J_new=0;
660         double c1_cond,c2_cond;
661         str[0] = (char*)"fail";
662         str[1] = (char*)"ok";
663
664         double J0 = J;
665         
666         for(int i=0; i < Options.max_iter; i++){
667                 dp=-B*dJ; //set change direction
668                 j=0;
669                 c1_cond = dJ*dp;
670                 c2_cond = dJ*(B*dJ);
671                 if(c1_cond < 0 && c2_cond> 0){
672                         while(true)
673                         {
674                                 alpha = 3*exp(-0.5*j); //set the step-length
675                                 p_tmp = p+dp*alpha; //change p 
676                                 J_new = Compute_J(dJ_new,p_tmp); //get corresponding J and dJ
677                                 j++;
678                                 
679                                 if( (J_new < J + dp*dJ*c1*alpha && dp*alpha*dJ_new>=dp*dJ*c2) || alpha < Options.tol_stopping*Options.tol_stopping || dJ.Norm()<Options.tol_stopping || fabs(c1_cond)<Options.tol_stopping)
680                                         break; //check if good enough, then break infinite loop
681                         }
682                 }
683                 else{
684                         printf("c1 and c2 conditons not satisfied\n");
685                         break;
686                 }
687                 
688                 if (Options.display){
689                         if (i%20 == 0){
690                                 printf("\n                                                    1st Optimality    2nd Optimality\n");
691                                 printf("   Iteration      J           |dJ|       step-size     Condition         Condition\n");
692                                 printf("---------------------------------------------------------------------------------------\n");
693                         }
694                         printf("      %-3i     %-10.3f    %-10.3f     %-4.3f          %-4s              %-4s\n",i+1,J,dJ.Norm(),alpha,str[c1_cond < 0],str[c2_cond > 0]);
695                 }
696         
697                 Compute_Hess(B,dJ_new-dJ,dp*alpha); //update the hessian (result in B)
698                 p = p + dp*alpha; //update p
699                 J=J_new;
700                 displayData.push_back(J_new);
701                 //PaintData(displayData);
702                 dJ=dJ_new;
703                 if (alpha < Options.tol_stopping*Options.tol_stopping || dJ.Norm()<Options.tol_stopping || fabs(c1_cond)<Options.tol_stopping)
704                         break; //break before max_iteration if good enough
705         }
706         
707         Unpack_params(p); //forming Priors, Mu, and Sigma from parameters p
708         return true;
709 }
710
711
712 /* This function computes the sensitivity of Cost function w.r.t. optimization parameters.
713  * The result is saved in the Vector dJ. The returned value of function is J.
714  * Don't mess with this function. Very sensitive and a bit complicated!
715 */
716 double SEDS::Compute_J(Vector& dJ,Vector pp) //compute the objective function and derivative w.r.t parameters (updated in vector dJ) 
717 {
718         
719                 double J = 0; 
720                 double sum=0; 
721                 Vector col(2*d); // a temporary vector needed below
722                 int lastind=0;
723                 
724                 for (int k=0; k<K; k=k++){
725                         //constructing Priors
726                         Priors[k]=exp(pp[k]); //extract the Priors from correspondng position in optimization vector
727                         sum+=Priors(k);
728                         
729                         //reconstructing Sigma
730                         for(int j=0; j<2*d; j++){ //for all dimensions
731                                 col.Zero();
732                                 for(int i=j; i<2*d; i++){
733                                         col(i)=pp(K+K*d+lastind);
734                                         lastind++;
735                                 }
736                                 L[k].SetColumn(col, j);
737                         }
738                         Sigma[k]=L[k]*(L[k].Transpose());
739                         
740                         for(int i=0; i<2*d;i++)
741                                 Sigma[k](i,i)+=Options.tol_mat_bias;
742                         
743                         invSigma_x[k]=Sigma[k].GetMatrix(0,d-1,0,d-1).Inverse(&detSigma_x[k]);   
744                         Sigma_xdx[k]=Sigma[k].GetMatrix(d,2*d-1,0,d-1);
745                         
746                         invSigma[k]=Sigma[k].Inverse(&detSigma(k));
747                         
748                         A[k]=Sigma_xdx[k]*invSigma_x[k]; //dynamical system matrix. 
749                         
750                         //reconstructing Mu
751                         Mu_x[k] = pp.GetSubVector(K+k*d,d);
752                         Mu_xd[k] = A[k]*Mu_x[k]; //proagate the centers through the dynamical system
753                         for (int i=0; i<d; i++)
754                         {
755                                 Mu(i,k) = Mu_x[k](i);
756                                 Mu(i+d,k) = Mu_xd[k](i);
757                         }
758                 }
759                 Priors /= sum; //normalizing Priors
760                 
761                 //computing likelihood:
762                 double prob_tmp;
763                 Pxi_Priors.Zero();
764                         
765                 
766                 for (int k=0; k<K; k++){
767                         int d_tmp;
768                         double tmp_den;
769                         REALTYPE *p_X;
770                         
771                         if (Options.objective){ //likelihod
772                                 tmp_den = sqrt(pow(2*M_PI,2*d)*fabs(detSigma[k])+DBL_MIN);
773                                 d_tmp = 2*d;
774                                 p_X = Data.Array();
775                         }
776                         else{ //mse
777                                 tmp_den = sqrt(pow(2*M_PI,d)*fabs(detSigma_x[k])+DBL_MIN);
778                                 d_tmp = d;
779                                 p_X = X.Array();
780                         }
781                         
782                         REALTYPE *p_tmpData = tmpData[k].Array();
783                         for(int j=0; j<d_tmp; j++){
784                                 double tmp_dbl = Mu(j,k);
785                                 for(int i=0; i<nData; i++)
786                                         *p_tmpData++ = (*p_X++) - tmp_dbl;
787                         }
788                         
789                         if (Options.objective){ //likelihod
790                                 tmp_mat = invSigma[k]*tmpData[k];
791                         }
792                         else{ //mse
793                                 tmp_mat = invSigma_x[k]*tmpData[k];
794                         }
795                         
796                         REALTYPE *p_tmp_mat = tmp_mat.Array();
797                         REALTYPE *p_Pxi = Pxi[k].Array();
798                         REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();
799                         p_tmpData = tmpData[k].Array();
800                         prob.Zero();
801                         
802                         for(int i=0; i<d_tmp; i++){
803                                 REALTYPE *p_prob = prob.Array();
804                                 for(int j=0; j<nData; j++){
805                                         if (i<d_tmp-1){
806                                                 *p_prob++ += (*p_tmp_mat++) * (*p_tmpData++);
807                                         }
808                                         else{
809                                                 *p_prob += (*p_tmp_mat++) * (*p_tmpData++);
810                                                 if (*p_prob > 200) //to avoid numerical issue
811                                                         *p_prob = 200;
812                                                 else if (*p_prob < -200) //to avoid numerical issue
813                                                         *p_prob = -200;
814                                                         
815                                                 *p_Pxi = exp(-0.5*(*p_prob++))/tmp_den;
816                                                 *(p_Pxi_Priors++) += (*p_Pxi++)*Priors[k];
817                                         }
818                                 }
819                         }
820                         /*
821                         for(int j=0; j<d; j++)
822                                 tmpData[k].SetRow(Mu(j,k),j);
823                         
824                         tmpData[k]=X-tmpData[k]; // remove centers from data
825                         
826                         prob = ((invSigma_x[k]*tmpData[k])^tmpData[k]).SumRow();//cf the expontential in gaussian pdf
827                                 
828                         double tmp_den = sqrt(pow(2*M_PI,d)*fabs(detSigma_x[k])+DBL_MIN);
829                                 
830                         for(int j=0; j<nData; j++){
831                                 if (prob[j] > 200) //to avoid numerical issue
832                                         prob[j] = 200;
833                                 else if (prob[j] < -200) //to avoid numerical issue
834                                         prob[j] = -200;
835                                         
836                                 Pxi[k][j] = exp(-0.5*prob[j])/tmp_den;
837                                 Pxi_Priors[j] += Pxi[k][j]*Priors[k];
838                         }
839                         */ 
840                 }
841                 
842                 //computing GMR
843                 if (!Options.objective){ //mse
844                         for (int k=0; k<K; k++){
845                                 
846                                 tmp_mat = A[k]*X;
847                                 REALTYPE *p_tmp_mat = tmp_mat.Array();
848                                 REALTYPE *p_Xd_hat = Xd_hat.Array();
849                                 REALTYPE *p_Pxi = Pxi[k].Array();
850                                 REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();
851                                 
852                                 for(int i=0; i<d; i++)
853                                 {
854                                         REALTYPE *p_h = h[k].Array();
855
856                                         for(int j=0; j<nData; j++){
857                                                 if (i==0)
858                                                         *p_h = *(p_Pxi++)/(*(p_Pxi_Priors++)) * Priors[k];
859                                                 if (k==0)
860                                                         *(p_Xd_hat++) = *(p_h++) * (*p_tmp_mat++);
861                                                 else
862                                                         *(p_Xd_hat++) += *(p_h++) * (*p_tmp_mat++);
863                                         }
864                                 }
865                                 
866                                 /*
867                                 h[k] = Pxi[k]*Priors[k]/Pxi_Priors;
868                                 if (k==0)
869                                         Xd_hat = Vector_Multiply(tmp_mul,h[k])^(A[k]*X);
870                                 else
871                                         Xd_hat += Vector_Multiply(tmp_mul,h[k])^(A[k]*X);
872                                 */
873                         }
874                 }
875                 
876                 for(int i=0; i<d; i++)
877                         tmp_A(i,i) = 1; 
878                 
879                 double det_term;
880                 double term;
881                 
882                 Vector dJ_dMu_k(d);
883                 
884                 int i_c;
885                 dJ.Zero();
886                 for(int k=0; k<K; k++){
887                         //sensitivity wrt Priors
888                         if (Options.perior_opt)
889                         {
890                                 if (Options.objective){ //likelihood
891                                         REALTYPE *p_h = h[k].Array();
892                                         REALTYPE *p_Pxi = Pxi[k].Array();
893                                         REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();
894                                         sum = 0;
895                                         
896                                         for (int j=0; j<nData; j++){
897                                                 *p_h = (*p_Pxi++)/(*p_Pxi_Priors++)*Priors[k];
898                                                 sum += (*p_h++) - Priors[k];
899                                         }
900                                         
901                                         dJ[k] = -exp(pp(k))/Priors[k]*sum;
902                                 
903                                         /*
904                                         h[k] = Pxi[k]*Priors[k]/Pxi_Priors;
905                                         dJ(k)=-exp(pp(k))/Priors[k]*((h[k]-Priors[k]).Sum());
906                                         */
907                                 }
908                                 else{ //mse
909                                         
910                                         sum = 0;
911                                         double tmp_dbl;
912                                         tmp_mat = A[k]*X;
913                                         h_tmp[k].Zero(); //??
914                                         REALTYPE *p_tmp_mat = tmp_mat.Array();
915                                         REALTYPE *p_Xd_hat = Xd_hat.Array();
916                                         REALTYPE *p_Xd = Xd.Array();
917                                         
918                                         for (int i=0; i<d; i++){
919                                                 REALTYPE *p_h_tmp = h_tmp[k].Array();
920                                                 REALTYPE *p_h = h[k].Array();
921                                                 for (int j=0; j<nData; j++){
922                                                         tmp_dbl = *(p_h++) * (*(p_tmp_mat++)-*p_Xd_hat) * (*(p_Xd_hat++)-*(p_Xd++));
923                                                         *(p_h_tmp++) += tmp_dbl;
924                                                         sum += tmp_dbl;
925                                                 }
926                                         }
927                                         dJ[k] = exp(pp[k])/Priors[k]*sum;
928                                         
929                                         /*
930                                         h_tmp[k] = h[k]^(((A[k]*X-Xd_hat)^(Xd_hat-Xd)).SumRow()); //This vector is common in all dJ computation.
931                                                                                                                                                           //Thus, I defined it as a variable to save some computation power
932                                         dJ(k)= exp(pp(k))/Priors[k]*h_tmp[k].Sum();     //derivative of priors(k) w.r.t. p(k)
933                                         */
934                                 }
935                                 
936                         }
937                         
938                         if (Options.mu_opt)
939                         {
940                                 if (Options.objective){ //likelihood
941                                         tmp_A.InsertSubMatrix(0,d,A[k].Transpose(),0,d,0,d); // eq to Matlab [eye(2) A(:,:,i)']
942                                         dJ_dMu_k=-((tmp_A*invSigma[k])*tmpData[k])*h[k];
943                                         dJ.InsertSubVector(K+k*d,dJ_dMu_k,0,d);
944                                 }
945                                 else{ //mse
946                                         
947                                         tmp_mat = invSigma_x[k]*tmpData[k];
948                                         REALTYPE *p_tmp_mat = tmp_mat.Array();
949                                         REALTYPE *p_dJ = &dJ(K+k*d);
950                                         
951                                         for (int i=0; i<d; i++){
952                                                 REALTYPE *p_h_tmp = h_tmp[k].Array();
953                                                 for (int j=0; j<nData; j++)
954                                                         *p_dJ += *(p_tmp_mat++) * (*(p_h_tmp++));
955                                                 p_dJ++;
956                                         }
957                                                 
958                                         /*
959                                         dJ_dMu_k = (tmpData[k]*invSigma_x[k]).Transpose()*h_tmp[k];
960                                         dJ.InsertSubVector(K+k*d,dJ_dMu_k,0,d);
961                                         */
962                                 }
963                         }
964                         
965                         //sensitivity wrt sigma
966                         i_c=0;
967                         
968                         if (Options.objective) //likelihood
969                                 det_term = (detSigma(k)<0) ? -1:1; //det_term=sign(det_term)
970                         else
971                                 det_term = (detSigma_x(k)<0) ? -1:1; //det_term=sign(det_term)
972                         
973                         for (int i=0;i<2*d;i++){
974                                 for (int j=i;j<2*d;j++){
975                                         i_c = i_c + 1;
976                                         if (Options.sigma_x_opt || i>=d || j>=d)
977                                         {
978                                                 rSrs = rSrs *0;
979                                                 rSrs(j,i)=1;
980                                                 rSrs = rSrs*L[k].Transpose() + L[k]*rSrs.Transpose();
981                                                 
982                                                 if (Options.objective) {//likelihood
983                                                         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];
984                                                         tmp_mat = (invSigma[k]*(rSrs*invSigma[k]))*tmpData[k];
985                                                         double tmp_dbl = (-0.5)*det_term*(invSigma[k]*rSrs).Trace();
986                                                         Vector tmp_vec = invSigma[k].GetMatrix(0,2*d-1,d,2*d-1)*rAvrs;
987                                                         
988                                                         REALTYPE *p_tmp_mat = tmp_mat.Array();
989                                                         REALTYPE *p_tmpData = tmpData[k].Array();
990                                                         sum = 0;
991                                                         
992                                                         for (int i=0; i<2*d; i++){
993                                                                 REALTYPE *p_h = h[k].Array();
994                                                                 for (int j=0; j<nData; j++){
995                                                                         sum -= (0.5 * (*p_tmp_mat++) * (*p_tmpData) +
996                                                                                                   (i==0)*tmp_dbl + //derivative with respect to det Sigma which is in the numenator
997                                                                                                   (*p_tmpData++) * tmp_vec[i]) * (*p_h++); //since Mu_xi_d = A*Mu_xi, thus we should consider its effect here
998                                                                 }
999                                                         }
1000                                                         dJ(K+K*d+k*d*(2*d+1)+i_c-1) = sum;
1001                                                         
1002                                                         /*
1003                                                         Vector tmp_vec = (((invSigma[k]*(rSrs*invSigma[k]))*tmpData[k])^tmpData[k]).SumRow();
1004                                                         
1005                                                         dJ(K+K*d+k*d*(2*d+1)+i_c-1) = ( tmp_vec*0.5 +
1006                                                                         (-0.5)*det_term*(invSigma[k]*rSrs).Trace() + //derivative with respect to det Sigma which is in the numenator
1007                                                                         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
1008                                                                         *h[k]*(-1);
1009                                                         */ 
1010                                                 }
1011                                                 else { //mse
1012                                                         tmp_mat = (-A[k]*rSrs.GetMatrix(0,d-1,0,d-1) + rSrs.GetMatrix(d,2*d-1,0,d-1))*invSigma_x[k]*X;
1013                                                         Matrix tmp_mat2 = (invSigma_x[k]*rSrs.GetMatrix(0,d-1,0,d-1)*invSigma_x[k])*tmpData[k];
1014                                                         double tmp_dbl = -det_term*(invSigma_x[k]*rSrs.GetMatrix(0,d-1,0,d-1)).Trace();
1015                                                         
1016                                                         REALTYPE *p_tmp_mat = tmp_mat.Array();
1017                                                         REALTYPE *p_tmp_mat2 = tmp_mat2.Array();
1018                                                         REALTYPE *p_tmpData = tmpData[k].Array();
1019                                                         REALTYPE *p_Xd_hat = Xd_hat.Array();
1020                                                         REALTYPE *p_Xd = Xd.Array();
1021                                                         sum = 0;
1022                                                         
1023                                                         for (int i=0; i<d; i++){
1024                                                                 REALTYPE *p_h_tmp = h_tmp[k].Array();
1025                                                                 REALTYPE *p_h = h[k].Array();
1026                                                                 for (int j=0; j<nData; j++){
1027                                                                         sum += (*p_h++) * (*p_tmp_mat++) * ((*p_Xd_hat++) - (*p_Xd++)) +  //derivative of A
1028                                                                                    0.5 * (*p_h_tmp++) * ((*p_tmp_mat2++) * (*p_tmpData++) + (i == 0)*tmp_dbl); //derivative with respect to det Sigma which is in the numenator
1029                                                                                                                                                                                                         //the above term (i==d-1) is just to sum temp_dbl once
1030                                                                 }
1031                                                         }
1032                                                         dJ(K+K*d+k*d*(2*d+1)+i_c-1) = sum;
1033                                                                                                                           
1034                                                         /*
1035                                                         dJ(K+K*d+k*d*(2*d+1)+i_c-1) =  (
1036                                                                 (h[k]^((((-A[k]*rSrs.GetMatrix(0,d-1,0,d-1) + rSrs.GetMatrix(d,2*d-1,0,d-1))*invSigma_x[k]*X)^(Xd_hat-Xd)).SumRow())) +//derivative of A
1037                                                                 ((((invSigma_x[k]*rSrs.GetMatrix(0,d-1,0,d-1)*invSigma_x[k]*tmpData[k])^tmpData[k]).SumRow()  + //derivative w.r.t. Sigma in exponential
1038                                                                      -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
1039                                                                 ).Sum();
1040                                                         */
1041                                                 }
1042                                         }
1043                                 }
1044                         }
1045                 }
1046                 
1047                 int i1_tmp;
1048                 double tmp_detB;
1049                 Vector detB(d);
1050                 c.Zero();
1051                 //constraints
1052                 for (int k=0; k<K; k++)//for all states
1053                 {
1054                         i1_tmp = 1; 
1055                         B = (A[k]+A[k].Transpose())/2; //define B
1056
1057                         //computing the constraints (this part is basicly an exact rewrite of the matlab version)
1058                         for (int i=0; i<d; i++) //for all dimensions
1059                         {
1060                                 B_Inv[i]=B.GetMatrix(0,i,0,i).Inverse(&tmp_detB);//get inverse and determinants of minors
1061                                 
1062                                 if (i1_tmp*tmp_detB+pow(Options.tol_mat_bias,(double)(i+1))/d > 0)
1063                                         c(k*d+i) = i1_tmp*tmp_detB+pow(Options.tol_mat_bias,(double)(i+1))/d;
1064
1065                                 //computing the sensitivity of the constraints to the parameters
1066                                 i_c = 0;
1067                                 for (int ii=0;ii<d;ii++){
1068                                         for (int jj=ii;jj<2*d;jj++){
1069                                                 if (Options.sigma_x_opt || ii>=d || jj>=d)
1070                                                 {
1071                                                         rSrs = rSrs * 0;
1072                                                         rSrs (jj,ii) = 1;
1073                                                         rSrs = rSrs*L[k].Transpose() + L[k]*rSrs.Transpose();
1074                                                         rArs = (-A[k] * rSrs.GetMatrix(0,d-1,0,d-1) + rSrs.GetMatrix(d,2*d-1,0,d-1)) * invSigma_x[k];
1075                                                         rBrs = (rArs + rArs.Transpose())/2;
1076                                                         if (i==0)
1077                                                                 term = rBrs(0,0);
1078                                                         else
1079                                                                 term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;
1080                                                         
1081                                                         dc(k*d+i,K + d*K + k*d*(2*d+1)+i_c) = i1_tmp*term;
1082                                                 }
1083                                                 i_c = i_c +1;
1084                                         }
1085                                 }
1086                                 i1_tmp *= -1; //used to alternate sign over iterations
1087                         }
1088                 }
1089                 
1090                 //calc J
1091                 if (Options.objective) {//likelihood
1092                         REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();
1093                         for(int i=0; i<nData ; i++)
1094                                 J -= log(*p_Pxi_Priors++);
1095                 }
1096                 else{
1097                         REALTYPE *p_Xd_hat = Xd_hat.Array();
1098                         REALTYPE *p_Xd = Xd.Array();
1099                         for(int i=0; i<d ; i++)
1100                                 for(int j=0; j<nData ; j++)
1101                                         J += 0.5 * ((*p_Xd_hat) - (*p_Xd)) * ((*p_Xd_hat++) - (*p_Xd++));
1102                         //J = 0.5*((Xd_hat-Xd)^(Xd_hat-Xd)).Sum();
1103                 }
1104                 
1105                 J = J/nData + Options.cons_penalty*(c*c);
1106                 dJ = dJ/nData + (dc.Transpose()*c)*2*Options.cons_penalty; //calc dJ
1107                 return J;
1108 }
1109
1110
1111 /*Updates the estimated value of inverse of Hessian at each iteration.
1112  * The function is written based on BFGS method.
1113 */
1114 bool SEDS::Compute_Hess(Matrix &B, Vector gamma, Vector delta){ //updates the hessian, B
1115         double gBg=gamma*(B*gamma);
1116         double dg=delta*gamma;
1117         Matrix dd=Vector_Multiply(delta,delta);
1118         Matrix gg=Vector_Multiply(gamma,gamma);
1119         Vector tmp1=(delta/dg-B*gamma/gBg);
1120         B = B + dd/dg - B * gg * B /gBg +   Vector_Multiply(tmp1,tmp1)*gBg;
1121         return true;
1122 }
1123
1124
1125 /* Transforming the vector of optimization's parameters into a GMM model.*/
1126 bool SEDS::Unpack_params(Vector pp){ //this is used to unpack the parameters in p after optimization, to reconstruct the 
1127 //GMM-parameters in their ususal form: Priors, Mu and Sigma. 
1128         
1129         int lastind=0;
1130         double sum=0;
1131         Vector Mu_x(d),Mu_xd(2*d),col(2*d);
1132         Matrix* A=new Matrix[K];
1133         
1134         for (int k=0; k<K; k=k++){
1135                 
1136                 //Constructin Priors
1137                 Priors(k)=exp(pp(k)); //extract the Priors from correspondng position in optimization vector
1138                 sum+=Priors(k);
1139                 
1140                 //constructing Sigma
1141                 for(int j=0; j<2*d; j++){ //for all dimensions
1142                         col.Zero();
1143                         for(int i=j; i<2*d; i++){
1144                                 col(i)=pp(K+K*d+lastind);
1145                                 lastind++;
1146                         }
1147                         L[k].SetColumn(col, j);
1148                 }
1149                 Sigma[k]=Matrix(2*d,2*d);
1150                 Sigma[k]=L[k]*(L[k].Transpose());
1151                 for(int i=0; i<2*d;i++)
1152                         Sigma[k](i,i)+=Options.tol_mat_bias;
1153                 
1154                 A[k]=Matrix(d,d);
1155                 A[k]=Sigma[k].GetMatrix(d,2*d-1,0,d-1)*Sigma[k].GetMatrix(0,d-1,0,d-1).Inverse(); //dynamical system matrix. 
1156                         
1157                 //reconstructing Mu
1158                 Mu_x = pp.GetSubVector(K+k*d,d);
1159                 Mu_xd = A[k]*Mu_x; //proagate the centers through the dynamical system
1160                 
1161                 for (int i=0; i<d; i++)
1162                 {
1163                         Mu(i,k) = Mu_x(i);
1164                         Mu(i+d,k) = Mu_xd(i);
1165                 }
1166         }
1167         
1168         Priors /= sum; //Normalizing Priors
1169         
1170         if (Options.display)
1171                 CheckConstraints(A);
1172                 
1173         return true;
1174 }
1175
1176 /* checking if every thing goes well. Sometimes if the parameter
1177  * 'Options.cons_penalty' is not big enough, the constrains may be violated.
1178  * Then this function notifies the user to increase 'Options.cons_penalty'.
1179 */
1180 bool SEDS::CheckConstraints(Matrix * A){
1181         bool ver=true;
1182         Vector eigvals;
1183         Matrix eigvects;
1184         Vector Mu_k(2*d);
1185         Matrix B(d,d);
1186         for(int k=0; k<K; k++)
1187         {
1188                 B = (A[k]+A[k].Transpose())/2;
1189                 B.EigenValuesDecomposition(eigvals,eigvects,100);
1190                 for(int i=0; i<eigvals.Size(); i++){
1191                         if(eigvals(i) > 0){
1192                                 if (ver)
1193                                 {
1194                                         cout << endl;
1195                                         cout<<"Optimization did not reach to an optimal point."<<endl;
1196                                         cout<<"Some constraints were slightly violated."<<endl;
1197                                         cout<<"The error may be due to change of hard constraints to soft constrints."<<endl;
1198                                         cout<<"To handle this error, increase the value of 'cons_penalty'"<<endl;
1199                                         cout<<"and re-run the optimization."<<endl<<endl;
1200                                         cout<<"Output error for debugging purpose:"<<endl;
1201                                 }
1202                                 cout<< "k = " << k << "  ;  err = " << eigvals(i) << endl;
1203                                 ver=false;
1204                         }
1205                 }
1206         }
1207         
1208         if(ver)
1209                 cout<<"Optimization finished successfully!"<<endl;
1210         cout << endl;
1211         
1212         return true;
1213 }
1214
1215 // Computes x1^T * x2
1216 Matrix SEDS::Vector_Multiply(Vector &x1, Vector &x2)
1217 {
1218         int rows=x1.Size();
1219         int cols=x2.Size();
1220         Matrix output(rows,cols);
1221         
1222         REALTYPE *p_x1 = x1.Array();
1223         REALTYPE *p_out = output.Array();
1224         
1225         for (int i=0; i<rows; i++)
1226         {
1227                 REALTYPE *p_x2 = x2.Array();
1228                 for (int j=0; j<cols; j++)
1229                         *(p_out++) = *p_x1 * (*(p_x2++));
1230                 p_x1++;
1231         }
1232                         
1233         return output;
1234 }