himrep
DictionaryLearning.cpp
1 #include "DictionaryLearning.h"
2 
3 using namespace std;
4 using namespace yarp::sig;
5 using namespace yarp::os;
6 using namespace yarp::math;
7 
8 DictionaryLearning::DictionaryLearning(string dictionariesFilePath, string group, string codingM, int K)
9 {
10  this->dictionariesFilePath=dictionariesFilePath;
11  this->group=group;
12  this->codingType=codingM;
13 
14  read();
15 
16  maxComparisons=350;
17  cv::Mat dict;
18 
19  dictionaryT=dictionary.transposed();
20  convert(dictionaryT,dict);
21  kdtree.build(dict);
22  if(usePCA)
23  {
24  feat.resize(dimPCA);
25  }
26  else
27  {
28  feat.resize(featuresize);
29  }
30  Knn=K;
31  rankIndices.resize(Knn);
32 
33  fprintf(stdout, "featureSize %d, dictionarySize %d, usePCA %s, dimPCA %d, KNN %d, powerNormalization %s, alpha %f, Mapping %s \n",featuresize,dictionarySize,(usePCA)?"true":"false",dimPCA, Knn, (powerNormalization)?"true":"false",alpha,mappingType.c_str());
34 
35 }
36 
37 bool DictionaryLearning::read()
38 {
39  Property config; config.fromConfigFile(dictionariesFilePath.c_str());
40  Bottle& bgeneral=config.findGroup("GENERAL");
41  lambda=bgeneral.find("lambda").asFloat64();
42  usePCA=bgeneral.check("usePCA",Value(0)).asInt32();
43  dimPCA=bgeneral.check("dimPCA",Value(80)).asInt32();
44 
45  powerNormalization=bgeneral.check("powerNormalization",Value(0)).asInt32();
46  alpha=bgeneral.check("alpha",Value(3)).asInt32(); // alpha-root
47  alpha=1./alpha;
48 
49 
50  mappingType=bgeneral.check("mappingType",Value("linear")).asString().c_str();
51 
52  dictionarySize=bgeneral.find("dictionarySize").asInt32();
53  Bottle& dictionaryGroup=config.findGroup(group.c_str());
54  featuresize=dictionaryGroup.find("featureSize").asInt32();
55  if(!usePCA)
56  dictionary.resize(featuresize,dictionarySize);
57  else
58  dictionary.resize(dimPCA,dictionarySize);
59 
60 
61  int fdim=usePCA? dimPCA:featuresize;
62 
63  for (int i=0; i<fdim; i++)
64  {
65  ostringstream oss;
66  oss << (i+1);
67  string num = "line"+oss.str();
68  Bottle* line=dictionaryGroup.find(num.c_str()).asList();
69 
70  for (int j=0; j<dictionarySize; j++)
71  dictionary(i,j)=line->get(j).asFloat64();
72  }
73 
74  double beta=1e-4;
75  Matrix trans=dictionary.transposed();
76  Matrix identity=eye(dictionarySize,dictionarySize);
77  Matrix diagonal=2*beta*identity;
78  A=trans*dictionary+diagonal;
79 
80 
81 
82  if(usePCA)
83  {
84  Bottle& eigengroup=config.findGroup("EIGENVECTORS");
85  yarp::sig::Matrix eig(dimPCA,featuresize);
86  for (int i=0; i<dimPCA; i++)
87  {
88  ostringstream oss;
89  oss << (i+1);
90  string num = "line"+oss.str();
91  Bottle* line=eigengroup.find(num.c_str()).asList();
92 
93  for (int j=0; j<featuresize; j++)
94  eig(i,j)=line->get(j).asFloat64();
95  }
96 
97  cv::Mat eigens;
98  convert(eig,eigens);
99  PCA.eigenvectors=eigens;
100 
101  eigengroup=config.findGroup("EIGENVALUES");
102  yarp::sig::Matrix eigv(dimPCA,1);
103  for (int i=0; i<dimPCA; i++)
104  {
105  ostringstream oss;
106  oss << (i+1);
107  string num = "line"+oss.str();
108  Bottle* line=eigengroup.find(num.c_str()).asList();
109 
110  eigv(i,0)=line->get(0).asFloat64();
111  }
112 
113  cv::Mat eigenvs;
114  convert(eigv,eigenvs);
115  PCA.eigenvalues=eigenvs;
116 
117  eigengroup=config.findGroup("MEAN");
118  yarp::sig::Matrix mean(1,featuresize);
119  string num = "line1";
120  Bottle* line=eigengroup.find(num.c_str()).asList();
121 
122  for (int j=0; j<featuresize; j++)
123  mean(0,j)=line->get(j).asFloat64();
124 
125 
126  cv::Mat m;
127  convert(mean,m);
128  PCA.mean=m;
129  }
130  return true;
131 }
132 
133 void DictionaryLearning::isBestElement(double score, int index, int numberOfBestPoints)
134 {
135  if (rankScores.size()==0)
136  {
137  rankScores.push_back(score);
138  rankIndices.push_back(index);
139  }
140  else if (rankScores.size()<numberOfBestPoints)
141  {
142  bool assigned=false;
143  std::vector<int>::iterator itind=rankIndices.begin();
144  for (std::vector<double>::iterator itsc = rankScores.begin(); itsc!=rankScores.end(); itsc++)
145  {
146  if (*itsc<score)
147  {
148  rankScores.insert(itsc,score);
149  rankIndices.insert(itind,index);
150  assigned=true;
151  break;
152  }
153  itind++;
154  }
155  if (!assigned)
156  {
157  rankScores.push_back(score);
158  rankIndices.push_back(index);
159  }
160  }
161  else
162  {
163  if (rankScores[rankScores.size()-1]>score)
164  return;
165  else if (rankScores[0]<score)
166  {
167  std::vector<double>::iterator itsc=rankScores.begin();
168  std::vector<int>::iterator itind=rankIndices.begin();
169  rankScores.insert(itsc,score);
170  rankIndices.insert(itind,index);
171  rankScores.pop_back();
172  rankIndices.pop_back();
173  }
174  else
175  {
176  std::vector<int>::iterator itind=rankIndices.begin();
177  for (std::vector<double>::iterator itsc = rankScores.begin(); itsc!=rankScores.end(); itsc++)
178  {
179  if (*itsc<score)
180  {
181  rankScores.insert(itsc,score);
182  rankIndices.insert(itind,index);
183  rankScores.pop_back();
184  rankIndices.pop_back();
185  break;
186  }
187  itind++;
188  }
189  }
190  }
191 }
192 
193 void DictionaryLearning::bestCodingEver_ANN(yarp::sig::Vector& feature, yarp::sig::Vector& descriptor)
194 {
195  if(usePCA)
196  {
197  cv::Mat mat(1,feature.size(),CV_64FC1,(double*) feature.data());
198  mat=PCA.project(mat);
199  feature.resize(dimPCA,0.0);
200  for (int i=0; i<feature.size(); i++)
201  feature[i]=mat.at<float>(0,i);
202  }
203 
204  descriptor.resize(dictionarySize,0.0);
205  for (int i=0; i<feature.size(); i++)
206  {
207  feat[i]=feature[i];
208  }
209 
210  kdtree.findNearest(feat,Knn,maxComparisons,rankIndices);
211 
212  for (int i=0; i<Knn; i++)
213  {
214  int id=rankIndices[i];
215 
216  yarp::sig::Vector v=dictionaryT.getRow(id);
217  double val;
218  if(mappingType=="linear")
219  val=yarp::math::dot(v,feature);
220 
221  if(mappingType=="additive_chisquare")
222  {
223  val=additive_chisquare(v, feature);
224  }
225 
226  if(mappingType=="gauss_chisquare")
227  {
228  val=gauss_chisquare(v, feature);
229  }
230  descriptor[id]=val;
231 
232  }
233 
234 
235 }
236 
237 
238 void DictionaryLearning::bestCodingEver(const yarp::sig::Vector& feature, yarp::sig::Vector& descriptor)
239 {
240  rankScores.clear();
241  rankIndices.clear();
242  Vector code=dictionaryT*feature;
243  descriptor.resize(code.size(),0.0);
244 
245  for (int i=0; i<code.size(); i++)
246  {
247  isBestElement(code[i], i, Knn);
248  }
249 
250  for (int i=0; i<Knn; i++)
251  {
252  int id=rankIndices[i];
253  descriptor[id]=rankScores[i];
254  }
255 
256 }
257 
258 
259 
260 
261 
262 double DictionaryLearning::customNorm(const yarp::sig::Vector &A,const yarp::sig::Vector &B)
263 {
264  double norm=0;
265 
266  for (int i=0; i<128; i++)
267  {
268  double d=A[i]-B[i];
269  norm+=d*d;
270 
271  }
272 
273 
274  return norm;
275 
276 }
277 
278 void DictionaryLearning::bow(yarp::sig::Vector & feature, yarp::sig::Vector & code)
279 {
280 
281  if(usePCA)
282  {
283  cv::Mat mat(1,feature.size(),CV_64FC1,(double*) feature.data());
284  mat=PCA.project(mat);
285  feature.resize(dimPCA,0.0);
286  for (int i=0; i<feature.size(); i++)
287  feature[i]=mat.at<float>(0,i);
288  }
289 
290  //transform to array of float
291  fprintf(stdout, "Starting bow coding \n");
292  double time=Time::now();
293  code.resize(dictionarySize);
294  code=0.0;
295 
296 
297  float minNorm=1e20;
298  int winnerAtom;
299 
300 
301  for(int atom_idx=0; atom_idx<dictionarySize; atom_idx++)
302  {
303  yarp::sig::Vector v=dictionary.getCol(atom_idx);
304  float norm=0.0f;
305  for (int i=0; i<featuresize; i++)
306  {
307  float d=feature[i]-v[i];
308  norm+=d*d;
309  if (norm > minNorm)
310  break;
311 
312  }
313  if(norm<minNorm)
314  {
315  minNorm=norm;
316  winnerAtom=atom_idx;
317  }
318 
319 
320  }
321 
322  code[winnerAtom]++;
323  time=Time::now()-time;
324  fprintf(stdout, "%f \n",time);
325 
326 
327 
328 
329 }
330 
331 
332 void DictionaryLearning::avgPooling(std::vector<yarp::sig::Vector> & features, yarp::sig::Vector & code, vector<SiftGPU::SiftKeypoint> & keypoints, int pLevels, int imgW, int imgH)
333 {
334  //fprintf(stdout, "Starting max pooling \n");
335  double time=Time::now();
336  vector<Vector> allCodes;
337  allCodes.resize(features.size());
338 
339  for (int i=0; i<features.size(); i++)
340  {
341  Vector &current=features[i];
342 
343 
344 
345  Vector currentCode;
346  if(codingType=="SC")
347  computeSCCode(current,currentCode);
348  if(codingType=="BCE")
349  bestCodingEver_ANN(current,currentCode);
350  if(codingType=="BOW")
351  bow(current,currentCode);
352  allCodes[i]=currentCode;
353  }
354 
355  //fprintf(stdout, "Finished \n");
356 
357  int sizeF=allCodes[0].size();
358  int sizeS=allCodes.size();
359 
360 
361  vector<int> pBins(pLevels);
362  vector<int> pyramid(pLevels);
363 
364  pBins[0]=1;
365  pyramid[0]=1;
366  int tBins=1;
367 
368  for (int i=1; i<pLevels; i++)
369  {
370  pyramid[i]=i*2;
371  pBins[i]=(i*2)*(i*2);
372  tBins+=pBins[i];
373  }
374 
375 
376 
377  code.resize(sizeF*tBins);
378  int binId=0;
379  //cvZero(debugImg);
380  for (int iter=0; iter<pLevels; iter++)
381  {
382  //fprintf(stdout, "Iter: %d \n", iter);
383  int nBins=pyramid[iter];
384  int wUnit= imgW/pyramid[iter];
385  int hUnit= imgH/pyramid[iter];
386 
387  //fprintf(stdout, "tBins: %d nBins: %d imgW: %d imgH: %d \n", tBins, nBins,wUnit,hUnit);
388 
389  for (int iterBinW=0; iterBinW<nBins; iterBinW++)
390  {
391  for (int iterBinH=0; iterBinH<nBins; iterBinH++)
392  {
393  int currBinW=wUnit*(iterBinW+1);
394  int currBinH=hUnit*(iterBinH+1);
395  int prevBinW=wUnit*iterBinW;
396  int prevBinH=hUnit*iterBinH;
397 
398  double normalizer=0.0;
399  int start=binId;
400  for (int i=0; i<sizeF; i++)
401  {
402  double value=0.0;
403  int total=0;
404  for (int k=0; k<sizeS; k++)
405  {
406 
407  if(keypoints[k].x< prevBinW || keypoints[k].x>= currBinW || keypoints[k].y< prevBinH || keypoints[k].y>= currBinH)
408  continue;
409 
410  /*if(iter==2 && iterBinW==3 && iterBinH==3)
411  {
412  int x = cvRound(keypoints[k].x);
413  int y = cvRound(keypoints[k].y);
414  cvCircle(debugImg,cvPoint(x,y),2,cvScalar(255),2);
415  //fprintf(stdout,"Feature X %f: Feature Y: %f \n", keypoints[k].x,keypoints[k].y);
416  }*/
417  value=value+allCodes[k][i];
418  total++;
419  }
420  if(total>0)
421  {
422  code[binId]=value/total;
423  normalizer=normalizer+((value/total)*(value/total));
424  }
425  binId++;
426  }
427 
428  // Power Normalization
429  if(powerNormalization)
430  {
431  if(normalizer>0)
432  normalizer=sqrt(normalizer);
433  {
434  for (int i=start; i<binId; i++)
435  {
436  code[i]=code[i]/normalizer;
437  int sign = code[i]>0? 1:-1;
438  code[i]=sign*pow(abs(code[i]),alpha);
439  }
440  }
441  }
442 
443  }
444  }
445 
446  }
447 
448  if(!powerNormalization)
449  {
450  double norm= yarp::math::norm(code);
451  code=code/norm;
452  }
453 
454  time=Time::now()-time;
455  //fprintf(stdout, "%f \n",time);
456 }
457 
458 
459 void DictionaryLearning::maxPooling(std::vector<yarp::sig::Vector> & features, yarp::sig::Vector & code, vector<SiftGPU::SiftKeypoint> & keypoints, int pLevels, int imgW, int imgH)
460 {
461  //fprintf(stdout, "Starting max pooling \n");
462  double time=Time::now();
463  vector<Vector> allCodes;
464  allCodes.resize(features.size());
465 
466  for (int i=0; i<features.size(); i++)
467  {
468  Vector &current=features[i];
469 
470 
471 
472  Vector currentCode;
473  if(codingType=="SC")
474  computeSCCode(current,currentCode);
475  if(codingType=="BCE")
476  bestCodingEver_ANN(current,currentCode);
477  if(codingType=="BOW")
478  bow(current,currentCode);
479  allCodes[i]=currentCode;
480  }
481 
482  //fprintf(stdout, "Finished \n");
483 
484  int sizeF=allCodes[0].size();
485  int sizeS=allCodes.size();
486 
487 
488  vector<int> pBins(pLevels);
489  vector<int> pyramid(pLevels);
490 
491  pBins[0]=1;
492  pyramid[0]=1;
493  int tBins=1;
494 
495  for (int i=1; i<pLevels; i++)
496  {
497  pyramid[i]=i*2;
498  pBins[i]=(i*2)*(i*2);
499  tBins+=pBins[i];
500  }
501 
502 
503 
504  code.resize(sizeF*tBins);
505  int binId=0;
506  //cvZero(debugImg);
507  for (int iter=0; iter<pLevels; iter++)
508  {
509  //fprintf(stdout, "Iter: %d \n", iter);
510  int nBins=pyramid[iter];
511  int wUnit= imgW/pyramid[iter];
512  int hUnit= imgH/pyramid[iter];
513 
514  //fprintf(stdout, "tBins: %d nBins: %d imgW: %d imgH: %d \n", tBins, nBins,wUnit,hUnit);
515 
516  for (int iterBinW=0; iterBinW<nBins; iterBinW++)
517  {
518  for (int iterBinH=0; iterBinH<nBins; iterBinH++)
519  {
520  int currBinW=wUnit*(iterBinW+1);
521  int currBinH=hUnit*(iterBinH+1);
522  int prevBinW=wUnit*iterBinW;
523  int prevBinH=hUnit*iterBinH;
524 
525  double normalizer=0.0;
526  int start=binId;
527  for (int i=0; i<sizeF; i++)
528  {
529  double maxVal=-1000.0;
530  for (int k=0; k<sizeS; k++)
531  {
532 
533  if(keypoints[k].x< prevBinW || keypoints[k].x>= currBinW || keypoints[k].y< prevBinH || keypoints[k].y>= currBinH)
534  continue;
535 
536  /*if(iter==2 && iterBinW==3 && iterBinH==3)
537  {
538  int x = cvRound(keypoints[k].x);
539  int y = cvRound(keypoints[k].y);
540  cvCircle(debugImg,cvPoint(x,y),2,cvScalar(255),2);
541  //fprintf(stdout,"Feature X %f: Feature Y: %f \n", keypoints[k].x,keypoints[k].y);
542  }*/
543  double val=allCodes[k][i];
544  if(val>maxVal)
545  maxVal=val;
546  }
547  code[binId]=maxVal;
548  normalizer=normalizer+maxVal*maxVal;
549  binId++;
550  }
551 
552  // Power Normalization
553  if(powerNormalization)
554  {
555  if(normalizer>0)
556  normalizer=sqrt(normalizer);
557  {
558  for (int i=start; i<binId; i++)
559  {
560  code[i]=code[i]/normalizer;
561  int sign = code[i]>0? 1:-1;
562  code[i]=sign*pow(abs(code[i]),alpha);
563  }
564  }
565  }
566 
567  }
568  }
569 
570  }
571 
572  if(!powerNormalization)
573  {
574  double norm= yarp::math::norm(code);
575  code=code/norm;
576  }
577 
578  time=Time::now()-time;
579  //fprintf(stdout, "%f \n",time);
580 }
581 
582 
583 void DictionaryLearning::computeSCCode(yarp::sig::Vector& feature, yarp::sig::Vector& descriptor)
584 {
585  if(usePCA)
586  {
587  cv::Mat mat(1,feature.size(),CV_64FC1,(double*) feature.data());
588  mat=PCA.project(mat);
589  feature.resize(dimPCA,0.0);
590  for (int i=0; i<feature.size(); i++)
591  feature[i]=mat.at<float>(0,i);
592  }
593 
594  double eps=1e-7;
595  Vector b=(-1)*feature*dictionary;
596 
597  Vector grad=b;
598  descriptor.resize(dictionarySize,0.0);
599  double maxValue; int index=-1;
600  max(grad,maxValue,index);
601 
602  while(true)
603  {
604  if (grad[index]>lambda+eps)
605  descriptor[index]=(lambda-grad[index])/A(index,index);
606  else if (grad[index]<-lambda-eps)
607  descriptor[index]=(-lambda-grad[index])/A(index,index);
608  else
609  {
610  double sum=0.0;
611  for (int j=0; j<descriptor.size(); j++)
612  sum+=descriptor[j];
613  if (sum==0.0)
614  break;
615  }
616 
617  while(true)
618  {
619  Matrix Aa;
620  Vector ba;
621  Vector xa;
622  Vector a;
623  Vector signes;
624 
625  for (int i=0; i<descriptor.size(); i++)
626  {
627  if (descriptor[i]!=0.0)
628  {
629  a.push_back(i);
630  ba.push_back(b[i]);
631  xa.push_back(descriptor[i]);
632  if (descriptor[i]>0)
633  signes.push_back(1);
634  else
635  signes.push_back(-1);
636  }
637  }
638 
639  subMatrix(A,a,Aa);
640 
641  Vector vect=(-lambda*signes)-ba;
642  Vector xnew=luinv(Aa)*vect;
643 
644  Vector vectidx;
645  Vector bidx;
646  Vector xnewidx;
647 
648  double sum=0.0;
649  for (int i=0; i<xnew.size(); i++)
650  {
651  if (xnew[i]!=0.0)
652  {
653  vectidx.push_back(vect[i]);
654  bidx.push_back(ba[i]);
655  xnewidx.push_back(xnew[i]);
656  sum+=abs(xnew[i]);
657  }
658  }
659 
660  double onew=dot((vectidx/2+bidx),xnewidx)+lambda*sum;
661 
662  Vector s;
663  bool changeSign=false;
664  for (int i=0; i<xa.size(); i++)
665  {
666  if (xa[i]*xnew[i]<=0)
667  {
668  changeSign=true;
669  s.push_back(i);
670  }
671  }
672 
673  if (!changeSign)
674  {
675  for (int i=0; i<a.size(); i++)
676  descriptor[a[i]]=xnew[i];
677  break;
678  }
679 
680  Vector xmin=xnew;
681  double omin=onew;
682 
683  Vector d=xnew-xa;
684  Vector t=d/xa;
685 
686  for (int i=0; i<s.size(); i++)
687  {
688  Vector x_s=xa-d/t[s[i]];
689  x_s[s[i]]=0.0;
690 
691  Vector x_sidx;
692  Vector baidx;
693  Vector idx;
694  sum=0.0;
695  for (int j=0; j<x_s.size(); j++)
696  {
697  if (x_s[j]!=0)
698  {
699  idx.push_back(j);
700  sum+=abs(x_s[j]);
701  x_sidx.push_back(x_s[j]);
702  baidx.push_back(ba[j]);
703  }
704  }
705  Matrix Atmp2;
706  subMatrix(Aa,idx,Atmp2);
707 
708  Vector otmp=Atmp2*(x_sidx/2)+baidx;
709  double o_s=dot(otmp,x_sidx)+lambda*sum;
710 
711  if (o_s<omin)
712  {
713  xmin=x_s;
714  omin=o_s;
715  }
716  }
717 
718  for (int i=0; i<a.size(); i++)
719  descriptor[a[i]]=xmin[i];
720  }
721  //grad=A*descriptor+b;
722 
723  // We can perform this operation since the matrix A is symmetric! 10 times faster
724  grad=descriptor;
725  grad*=A;
726  grad+=b;
727 
728  Vector tmp;
729 
730  for (int i=0; i<descriptor.size(); i++)
731  {
732  if (descriptor[i]==0.0)
733  tmp.push_back(grad[i]);
734  else
735  tmp.push_back(0);
736  }
737  max(tmp,maxValue,index);
738 
739  if (maxValue<=lambda+eps)
740  break;
741  }
742 }
743 
744 void DictionaryLearning::subMatrix(const Matrix& A, const Vector& indexes, Matrix& Atmp)
745 {
746  int indSize=indexes.size();
747  Atmp.resize(indSize,indSize);
748  int m=0;
749  int n=0;
750  for (int i=0; i<indSize; i++)
751  {
752  for (int j=0; j<indSize; j++)
753  {
754  Atmp(m,n)=A(indexes[i],indexes[j]);
755  n++;
756  }
757  m++;
758  n=0;
759  }
760 }
761 
762 void DictionaryLearning::max(const Vector& x, double& maxValue, int& index)
763 {
764  maxValue=-1000;
765  for (int i=0; i<x.size(); i++)
766  {
767  if(abs((double)x[i])>maxValue)
768  {
769  maxValue=abs(x[i]);
770  index=i;
771  }
772  }
773 }
774 vector<vector<double> > DictionaryLearning::readFeatures(std::string filePath)
775 {
776  vector<vector<double> > featuresMat;
777 
778  string line;
779  ifstream infile;
780  infile.open (filePath.c_str());
781  while(!infile.eof() && infile.is_open()) // To get you all the lines.
782  {
783  vector<double> f;
784  getline(infile,line); // Saves the line in STRING.
785 
786  char * val= strtok((char*) line.c_str()," ");
787 
788  while(val!=NULL)
789  {
790 
791  double value=atof(val);
792  f.push_back(value);
793  val=strtok(NULL," ");
794  }
795  if(f.size()>0)
796  featuresMat.push_back(f);
797  }
798  infile.close();
799 
800 
801  return featuresMat;
802 }
803 void DictionaryLearning::learnDictionary(string featureFile, int dictionarySize, bool usePCA, int dim)
804 {
805  dimPCA=dim;
806  this->usePCA=usePCA;
807 
808  vector<vector<double> > Features=readFeatures(featureFile);
809  if(Features.size()==0)
810  return;
811 
812  this->featuresize=Features[0].size();
813 
814  cv::Mat samples(Features.size(), featuresize,CV_32F);
815 
816 
817  for (int i=0; i<Features.size(); i++)
818  {
819  for (int j=0; j<Features[i].size(); j++)
820  {
821  samples.at<float>(i,j)=(float)Features[i][j];
822  }
823  }
824 
825  if(usePCA)
826  {
827  fprintf(stdout, "Using PCA with dim: %d \n", dimPCA);
828  PCA(samples,cv::Mat(), CV_PCA_DATA_AS_ROW,dimPCA);
829  samples=PCA.project(samples);
830  }
831 
832  int feaSize=usePCA? dimPCA:featuresize;
833  cv::Mat labels;
834  cv::Mat centroids(dictionarySize,feaSize,CV_32F);
835  fprintf(stdout, "Learning Dictionary with %d atoms. \n", dictionarySize);
836 
837  //cv::kmeans(samples,dictionarySize,labels, cv::TermCriteria(),3,cv::KMEANS_PP_CENTERS , &centroids);
838  cv::kmeans(samples,dictionarySize,labels, cv::TermCriteria(),3,cv::KMEANS_PP_CENTERS , centroids);
839 
840  dictionary.resize(feaSize,dictionarySize);
841 
842  if(!usePCA)
843  {
844  for(int j=0; j<dictionary.cols(); j++)
845  {
846  for (int i=0; i<dictionary.rows(); i++)
847  {
848  dictionary(i,j)=centroids.at<float>(j,i);
849  }
850 
851  Vector col =dictionary.getCol(j);
852  double normVal=norm(col);
853 
854  if(normVal==0)
855  continue;
856 
857  for (int i=0; i<dictionary.rows(); i++)
858  {
859  dictionary(i,j)=dictionary(i,j)/normVal;
860  }
861  Vector col2 =dictionary.getCol(j);
862  normVal=norm(col2);
863  }
864  }
865  dictionaryT=dictionary.transposed();
866  double beta=1e-4;
867  Matrix trans=dictionary.transposed();
868  Matrix identity=eye(dictionarySize,dictionarySize);
869  Matrix diagonal=2*beta*identity;
870  A=trans*dictionary+diagonal;
871 
872  cv::Mat dict;
873  convert(dictionaryT,dict);
874  kdtree.build(dict);
875 
876  this->dictionarySize=dictionarySize;
877 
878  if(usePCA)
879  {
880  feat.resize(dimPCA);
881  }
882  else
883  {
884  feat.resize(featuresize);
885  }
886 
887 }
888 bool DictionaryLearning::saveDictionary(string filePath)
889 {
890  ofstream dictfile;
891  dictfile.open(filePath.c_str(),ios::trunc);
892 
893  dictfile << "[GENERAL] \n";
894  dictfile << "lambda " << lambda << "\n";
895  int use= usePCA? 1:0;
896  dictfile << "usePCA " << use << "\n";
897  dictfile << "dimPCA " << dimPCA << "\n";
898 
899  dictfile << "dictionarySize " << dictionaryT.rows() << "\n";
900 
901  dictfile << "[DICTIONARY] \n";
902  dictfile << "featureSize " << featuresize << "\n";
903 
904 
905  for (int i=0; i<dictionary.rows(); i++)
906  {
907  dictfile << "line" << i+1 << " ( ";
908  for (int j=0; j<dictionary.cols(); j++)
909  {
910  dictfile << dictionary(i,j) << " ";
911  }
912  dictfile << " ) \n";
913 
914  }
915 
916  if(usePCA)
917  {
918  dictfile << "[EIGENVECTORS] \n";
919  for (int i=0; i<PCA.eigenvectors.rows; i++)
920  {
921  dictfile << "line" << i+1 << " ( ";
922  for (int j=0; j<PCA.eigenvectors.cols; j++)
923  {
924  dictfile << PCA.eigenvectors.at<float>(i,j) << " ";
925  }
926  dictfile << " ) \n";
927 
928  }
929 
930  dictfile << "[EIGENVALUES] \n";
931  for (int i=0; i<PCA.eigenvalues.rows; i++)
932  {
933  dictfile << "line" << i+1 << " ( ";
934  for (int j=0; j<PCA.eigenvalues.cols; j++)
935  {
936  dictfile << PCA.eigenvalues.at<float>(i,j) << " ";
937  }
938  dictfile << " ) \n";
939 
940  }
941 
942  dictfile << "[MEAN] \n";
943  for (int i=0; i<PCA.mean.rows; i++)
944  {
945  dictfile << "line" << i+1 << " ( ";
946  for (int j=0; j<PCA.mean.cols; j++)
947  {
948  dictfile << PCA.mean.at<float>(i,j) << " ";
949  }
950  dictfile << " ) \n";
951 
952  }
953  }
954 
955 
956  dictfile.close();
957 
958  return true;
959 }
960 
961 void DictionaryLearning::printMatrixYarp(const yarp::sig::Matrix &A)
962 {
963  cout << endl;
964  for (int i=0; i<A.rows(); i++)
965  for (int j=0; j<A.cols(); j++)
966  cout<<A(i,j)<<" ";
967  cout<<endl;
968  cout << endl;
969 }
970 
971 
972 DictionaryLearning::~DictionaryLearning()
973 {
974  //write();
975 }
976 
977 void DictionaryLearning::convert(yarp::sig::Matrix& matrix, cv::Mat& mat) {
978  mat=cv::Mat(matrix.rows(),matrix.cols(),CV_32FC1);
979  for(int i=0; i<matrix.rows(); i++)
980  for(int j=0; j<matrix.cols(); j++)
981  mat.at<float>(i,j)=matrix(i,j);
982 }
983 
984 void DictionaryLearning::convert(cv::Mat& mat, yarp::sig::Matrix& matrix) {
985  matrix.resize(mat.rows,mat.cols);
986  for(int i=0; i<mat.rows; i++)
987  for(int j=0; j<mat.cols; j++)
988  matrix(i,j)=mat.at<double>(i,j);
989 }
990 
991 double DictionaryLearning::additive_chisquare(const yarp::sig::Vector &x, const yarp::sig::Vector &y){
992  double val=0;
993  for (int i=0; i<x.size(); i++)
994  {
995  double res=x[i]*y[i];
996  double sumAbs=(abs(x[i])+abs(y[i]));
997  if(sumAbs==0)
998  continue;
999  res=res/sumAbs;
1000  val=val+res;
1001  }
1002  return val;
1003 
1004 }
1005 
1006 double DictionaryLearning::gauss_chisquare(const yarp::sig::Vector &x, const yarp::sig::Vector &y){
1007  double val=0;
1008  for (int i=0; i<x.size(); i++)
1009  {
1010  double res=x[i]-y[i];
1011  int sign=(x[i]*y[i])>0? 1:-1;
1012  res=res*res;
1013  double sumAbs=(abs(x[i])+abs(y[i]));
1014  if(sumAbs==0)
1015  continue;
1016  res=res/sumAbs;
1017  val=val+(sign*exp(-(res*res)));
1018  }
1019  return val;
1020 
1021 }
Generated by doxygen 1.9.1