Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpTemplateTrackerMI.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Example of template tracking.
32  *
33  * Authors:
34  * Amaury Dame
35  * Aurelien Yol
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 #include <visp3/core/vpException.h>
40 #include <visp3/tt_mi/vpTemplateTrackerMI.h>
41 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
42 
43 void vpTemplateTrackerMI::setBspline(const vpBsplineType &newbs)
44 {
45  bspline=(int)newbs;
46  influBspline=bspline*bspline;
47  Ncb=Nc+bspline;
48  if (Pt) delete[] Pt;
49  if (Pr) delete[] Pr;
50  if (Prt) delete[] Prt;
51  if (dPrt) delete[] dPrt;
52  if (d2Prt) delete[] d2Prt;
53  if (PrtD) delete[] PrtD;
54  if (dPrtD) delete[] dPrtD;
55  if (PrtTout) delete[] PrtTout;
56 
57  Pt= new double[Ncb];
58  Pr= new double[Ncb];
59 
60  Prt= new double[Ncb*Ncb];
61  dPrt= new double[Ncb*Ncb*(int)(nbParam)];
62  d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];
63 
64  /*std::cout<<Nc*Nc*influBspline<<std::endl;std::cout<<Nc*Nc*nbParam*influBspline<<std::endl;*/
65  PrtD= new double[Nc*Nc*influBspline];
66  dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
67  PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
68 
69  hessianComputation=USE_HESSIEN_DESIRE;
70 }
71 
72 
73 vpTemplateTrackerMI::vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp)
74  : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0),
75  temp(NULL), Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL),
76  dprtemp(NULL), PrtD(NULL), dPrtD(NULL), influBspline(0), bspline(3), Nc(8), Ncb(0),
77  d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
78  NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
79 {
80  Ncb=Nc+bspline;
81  influBspline=bspline*bspline;
82 
83  dW.resize(2,nbParam);
84  H.resize(nbParam,nbParam);
85  G.resize(nbParam);
86  Hdesire.resize(nbParam,nbParam);
87  HLM.resize(nbParam,nbParam);
88  HLMdesire.resize(nbParam,nbParam);
89  dprtemp= new double[nbParam];
90  temp= new double[nbParam];
91 
92  X1.resize(2);
93  X2.resize(2);
94 
95  PrtD= new double[Nc*Nc*influBspline];//(r,t)
96  dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
97 
98  Prt= new double[Ncb*Ncb];//(r,t)
99  Pt= new double[Ncb];
100  Pr= new double[Ncb];
101  dPrt= new double[Ncb*Ncb*(int)(nbParam)];
102  d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];
103 
104  PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
105 
106  lambda=lambdaDep;
107 }
108 
109 void vpTemplateTrackerMI::setNc(int nc)
110 {
111  Nc=nc;
112  Ncb=Nc+bspline;
113 
114  if (Pt) delete[] Pt;
115  if (Pr) delete[] Pr;
116  if (Prt) delete[] Prt;
117  if (dPrt) delete[] dPrt;
118  if (d2Prt) delete[] d2Prt;
119  if (PrtD) delete[] PrtD;
120  if (dPrtD) delete[] dPrtD;
121  if (PrtTout) delete[] PrtTout;
122 
123  PrtD= new double[Nc*Nc*influBspline];//(r,t)
124  dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
125  Prt= new double[Ncb*Ncb];//(r,t)
126  dPrt= new double[Ncb*Ncb*(int)(nbParam)];
127  Pt= new double[Ncb];
128  Pr= new double[Ncb];
129  d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];//(r,t)
130  PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
131 }
132 
133 
134 double vpTemplateTrackerMI::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
135 {
136  double MI=0;
137  int Nbpoint=0;
138  double IW;
139 
140  unsigned int Ncb_ = (unsigned int) Ncb;
141  unsigned int Nc_ = (unsigned int) Nc;
142  unsigned int influBspline_ = (unsigned int) influBspline;
143 
144  memset(Prt, 0, Ncb_*Ncb_*sizeof(double));
145  memset(PrtD, 0, Nc_*Nc_*influBspline_*sizeof(double));
146 
147  //Warp->ComputeMAtWarp(tp);
148  Warp->computeCoeff(tp);
149  for(unsigned int point=0;point<templateSize;point++)
150  {
151  int i=ptTemplate[point].y;
152  int j=ptTemplate[point].x;
153  X1[0]=j;X1[1]=i;
154 
155  Warp->computeDenom(X1,tp);
156  Warp->warpX(X1,X2,tp);
157  double j2=X2[0];
158  double i2=X2[1];
159 
160  //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
161  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
162  {
163  Nbpoint++;
164 
165  double Tij=ptTemplate[point].val;
166  if(!blur)
167  IW=I.getValue(i2,j2);
168  else
169  IW=BI.getValue(i2,j2);
170 
171  int cr=(int)((IW*(Nc-1))/255.);
172  int ct=(int)((Tij*(Nc-1))/255.);
173  double er=(IW*(Nc-1))/255.-cr;
174  double et=((double)Tij*(Nc-1))/255.-ct;
175 
176  //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
177  vpTemplateTrackerMIBSpline::PutPVBsplineD(PrtD, cr, er, ct, et, Nc, 1.,bspline);
178  }
179  }
180 
181  ratioPixelIn=(double)Nbpoint/(double)templateSize;
182 
183  double *pt=PrtD;
184  for(int r=0;r<Nc;r++)
185  for(int t=0;t<Nc;t++)
186  {
187  for(int i=0;i<influBspline;i++)
188  {
189  int r2,t2;
190  r2=r+i/bspline;
191  t2=t+i%bspline;
192  Prt[r2*Ncb+t2]+=*pt;
193 
194  pt++;
195  }
196  }
197 
198  if(Nbpoint==0)
199  return 0;
200  for(unsigned int r=0;r<Ncb_;r++)
201  for(unsigned int t=0;t<Ncb_;t++)
202  //printf("%f ",Prt[r*Ncb+t]);
203  Prt[r*Ncb_+t]=Prt[r*Ncb_+t]/Nbpoint;
204  //calcul Pr;
205  memset(Pr, 0, Ncb_*sizeof(double));
206  for(unsigned int r=0;r<Ncb_;r++)
207  {
208  for(unsigned int t=0;t<Ncb_;t++)
209  Pr[r]+=Prt[r*Ncb_+t];
210  }
211 
212  //calcul Pt;
213  memset(Pt, 0, Ncb_*sizeof(double));
214  for(unsigned int t=0;t<Ncb_;t++)
215  {
216  for(unsigned int r=0;r<Ncb_;r++)
217  Pt[t]+=Prt[r*Ncb_+t];
218  }
219  for(unsigned int r=0;r<Ncb_;r++)
220  //if(Pr[r]!=0)
221  if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
222  MI-=Pr[r]*log(Pr[r]);
223 
224  for(unsigned int t=0;t<Ncb_;t++)
225  //if(Pt[t]!=0)
226  if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
227  MI-=Pt[t]*log(Pt[t]);
228 
229  for(unsigned int r=0;r<Ncb_;r++)
230  for(unsigned int t=0;t<Ncb_;t++)
231  //if(Prt[r*Ncb+t]!=0)
232  if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
233  MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
234 
235  return -MI;
236 }
237 
238 double vpTemplateTrackerMI::getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp)
239 {
240  // Attention, cette version calculée de la NMI ne pourra pas atteindre le maximum de 2
241  // Ceci est du au fait que par defaut, l'image est floutée dans vpTemplateTracker::initTracking()
242 
243  double MI=0;
244  double Nbpoint=0;
245  double IW;
246 
247  double Pr_[256];
248  double Pt_[256];
249  double Prt_[256][256];
250 
251  memset(Pr_, 0, 256*sizeof(double));
252  memset(Pt_, 0, 256*sizeof(double));
253  memset(Prt_, 0, 256*256*sizeof(double));
254 
255  for(unsigned int point=0;point<templateSize;point++)
256  {
257  int i=ptTemplate[point].y;
258  int j=ptTemplate[point].x;
259  X1[0]=j;X1[1]=i;
260 
261  Warp->computeDenom(X1,tp);
262  Warp->warpX(X1,X2,tp);
263  double j2=X2[0];
264  double i2=X2[1];
265 
266  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
267  {
268  Nbpoint++;
269  double Tij=ptTemplate[point].val;
270  if(!blur)
271  IW=I[(int)i2][(int)j2];
272  else
273  IW=BI.getValue(i2,j2);
274 
275  Pr_[(int)Tij]++;
276  Pt_[(int)IW]++;
277  Prt_[(int)Tij][(int)IW]++;
278  }
279  }
280 
281  for(int i = 0 ; i < 256 ; i++)
282  {
283  Pr_[i] /= Nbpoint;
284  Pt_[i] /= Nbpoint;
285  for(int j = 0 ; j < 256 ; j++)
286  Prt_[i][j] /= Nbpoint;
287  }
288 
289  for(int r=0;r<256;r++)
290  //if(Pr_[r]!=0)
291  if(std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon())
292  MI-=Pr_[r]*log(Pr_[r]);
293 
294  for(int t=0;t<256;t++)
295  //if(Pt_[t]!=0)
296  if(std::fabs(Pt_[t]) > std::numeric_limits<double>::epsilon())
297  MI-=Pt_[t]*log(Pt_[t]);
298 
299  double denom = 0;
300  for(int r=0;r<256;r++)
301  for(int t=0;t<256;t++)
302  //if(Prt_[r][t]!=0)
303  if(std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon())
304  denom-=(Prt_[r][t]*log(Prt_[r][t]));
305 
306  //if(denom != 0)
307  if(std::fabs(denom) > std::numeric_limits<double>::epsilon())
308  MI = MI/denom;
309  else MI = 0;
310 
311  return -MI;
312 }
313 
314 vpTemplateTrackerMI::~vpTemplateTrackerMI()
315 {
316  if (Pt) delete[] Pt;
317  if (Pr) delete[] Pr;
318  if (Prt) delete[] Prt;
319  if (dPrt) delete[] dPrt;
320  if (d2Prt) delete[] d2Prt;
321  if (PrtD) delete[] PrtD;
322  if (dPrtD) delete[] dPrtD;
323  if (PrtTout) delete[] PrtTout;
324  if (temp) delete[] temp;
325  if (dprtemp) delete[] dprtemp;
326 }
327 
328 void vpTemplateTrackerMI::computeProba(int &nbpoint)
329 {
330  double *pt=PrtTout;
331  unsigned int Nc_ = (unsigned int)Nc;
332  unsigned int Ncb_ = (unsigned int)Ncb;
333  unsigned int bspline_ = (unsigned int)bspline;
334 
335  for(unsigned int r=0;r<Nc_;r++) {
336  for(unsigned int t=0;t<Nc_;t++)
337  {
338  for(unsigned int r2=0;r2<bspline_;r2++) {
339  for(unsigned int t2=0;t2<bspline_;t2++)
340  {
341  Prt[((r2+r)*Ncb_+(t2+t))]+=*pt++;
342  for(unsigned int ip=0;ip<nbParam;ip++)
343  {
344  dPrt[((r2+r)*Ncb_+(t2+t))*nbParam+ip]+=*pt++;
345  for(unsigned int it=0;it<nbParam;it++){
346  d2Prt[((r2+r)*Ncb_+(t2+t))*nbParam*nbParam+ip*nbParam+it]+=*pt++;
347  }
348  }
349  }
350  }
351  }
352  }
353 
354  // for(unsigned int r=0;r<Nc;r++)
355  // for(unsigned int t=0;t<Nc;t++)
356  // {
357  // for(int r2=0;r2<bspline;r2++)
358  // for(int t2=0;t2<bspline;t2++)
359  // {
360  // Prt[((r2+r)*Ncb+(t2+t))]+=*pt++;
361  // for(int ip=0;ip<nbParam;ip++)
362  // {
363  // dPrt[((r2+r)*Ncb+(t2+t))*nbParam+ip]+=*pt++;
364  // }
365  // for(int ip=0;ip<nbParam;ip++)
366  // {
367  // for(int it=0;it<nbParam;it++)
368  // d2Prt[((r2+r)*Ncb+(t2+t))*nbParam*nbParam+ip*nbParam+it]+=*pt++;
369  // }
370  // }
371  // }
372 
373  // int vr2, vt2, vt2ip;
374  // for(unsigned int r=0;r<Nc;r++)
375  // for(unsigned int t=0;t<Nc;t++)
376  // {
377  // for(int r2=0;r2<bspline;r2++)
378  // {
379  // vr2 = (r2+r)*Ncb;
380  // for(int t2=0;t2<bspline;t2++)
381  // {
382  // vt2 = vr2+(t2+t);
383  // Prt[vt2]+=*pt++;
384  // vt2 *= nbParam;
385 
386  // for(int ip=0;ip<nbParam;ip++)
387  // dPrt[vt2+ip]+=*pt++;
388 
389  // for(int ip=0;ip<nbParam;ip++)
390  // {
391  // vt2ip = vt2*nbParam+ip*nbParam;
392  // for(int it=0;it<nbParam;it++)
393  // d2Prt[vt2ip+it]+=*pt++;
394  // }
395  // }
396  // }
397  // }
398 
399  if(nbpoint==0) {
400  //std::cout<<"plus de point dans template suivi"<<std::endl;
401  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
402  }
403  unsigned int indd, indd2;
404  indd = indd2 = 0;
405  for(int i=0;i<Ncb*Ncb;i++){
406  Prt[i]=Prt[i]/nbpoint;
407  for(unsigned int j=0;j<nbParam;j++){
408  dPrt[indd]=dPrt[indd]/nbpoint;
409  indd++;
410  for(unsigned int k=0;k<nbParam;k++){
411  d2Prt[indd2]=d2Prt[indd2]/nbpoint;
412  indd2++;
413  }
414  }
415  }
416 }
417 
418 void vpTemplateTrackerMI::computeMI(double &MI)
419 {
420  unsigned int Ncb_ = (unsigned int) Ncb;
421 
422  //calcul Pr;
423  memset(Pr, 0, Ncb_*sizeof(double));
424  for(unsigned int r=0;r<Ncb_;r++)
425  {
426  for(unsigned int t=0;t<Ncb_;t++)
427  Pr[r]+=Prt[r*Ncb_+t];
428  }
429 
430  //calcul Pt;
431  memset(Pt, 0, Ncb_*sizeof(double));
432  for(unsigned int t=0;t<Ncb_;t++)
433  {
434  for(unsigned int r=0;r<Ncb_;r++)
435  Pt[t]+=Prt[r*Ncb_+t];
436  }
437 
438  //calcul Entropies;
439  //double entropieI=0;
440  for(unsigned int r=0;r<Ncb_;r++)
441  {
442  //if(Pr[r]!=0)
443  if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
444  {
445  //entropieI-=Pr[r]*log(Pr[r]);
446  MI-=Pr[r]*log(Pr[r]);
447  }
448  }
449 
450  //double entropieT=0;
451  for(unsigned int t=0;t<Ncb_;t++)
452  {
453  //if(Pt[t]!=0)
454  if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
455  {
456  //entropieT-=Pt[t]*log(Pt[t]);
457  MI-=Pt[t]*log(Pt[t]);
458  }
459  }
460 
461  for(unsigned int r=0;r<Ncb_;r++)
462  for(unsigned int t=0;t<Ncb_;t++)
463  //if(Prt[r*Ncb+t]!=0)
464  if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
465  MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
466 }
467 
468 void vpTemplateTrackerMI::computeHessien(vpMatrix &Hessian)
469 {
470  double seuilevitinf=1e-200;
471  Hessian=0;
472  double dtemp;
473  unsigned int Ncb_ = (unsigned int)Ncb;
474  for(unsigned int t=0;t<Ncb_;t++)
475  {
476  //if(Pt[t]!=0)
477  if(Pt[t]>seuilevitinf)
478  {
479  for(unsigned int r=0;r<Ncb_;r++)
480  {
481  //if(Prt[r*Ncb+t]!=0)
482  if(Prt[r*Ncb_+t]>seuilevitinf)
483  {
484  for(unsigned int it=0;it<nbParam;it++)
485  dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
486 
487  dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
488  for(unsigned int it=0;it<nbParam;it++)
489  for(unsigned int jt=0;jt<nbParam;jt++)
490  if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_NEW)
491  Hessian[it][jt]+=dprtemp[it]*dprtemp[jt]*(1./Prt[r*Ncb_+t]-1./Pt[t])+d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*dtemp;
492  else if(ApproxHessian==HESSIAN_NEW)
493  Hessian[it][jt]+=d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*dtemp;
494  else
495  Hessian[it][jt]+=dprtemp[it]*dprtemp[jt]*(1./Prt[r*Ncb_+t]-1./Pt[t]);
496  /*std::cout<<"Prt[r*Ncb+t]="<<Prt[r*Ncb+t]<<std::endl;
497  std::cout<<"Pt[t]="<<Pt[t]<<std::endl;
498 
499  std::cout<<"Hessian="<<Hessian<<std::endl;*/
500  }
501  }
502  }
503  }
504  // std::cout<<"Hessian="<<Hessian<<std::endl;
505 }
506 
507 void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
508 {
509  double seuilevitinf=1e-200;
510  //double dtemp;
511  double u=0,v=0,B=0;
512  double *du = new double[nbParam];
513  double *dv = new double[nbParam];
514  double *A = new double[nbParam];
515  double *dB = new double[nbParam];
516  double **d2u = new double *[nbParam];
517  double **d2v = new double *[nbParam];
518  double **dA = new double *[nbParam];
519  for (unsigned int i = 0; i < nbParam; i++) {
520  d2u[i] = new double[nbParam];
521  d2v[i] = new double[nbParam];
522  dA[i] = new double[nbParam];
523  }
524 
525  memset(du, 0, nbParam*sizeof(double));
526  memset(dv, 0, nbParam*sizeof(double));
527  memset(A, 0, nbParam*sizeof(double));
528  memset(dB, 0, nbParam*sizeof(double));
529  memset(dprtemp, 0, nbParam*sizeof(double));
530 
531  for(unsigned int it=0;it<nbParam;it++){
532  for(unsigned int jt=0;jt<nbParam;jt++){
533  memset(d2u[it], 0, nbParam*sizeof(double));
534  memset(d2v[it], 0, nbParam*sizeof(double));
535  memset(dA[it], 0, nbParam*sizeof(double));
536  }
537  }
538 
539  unsigned int Ncb_ = (unsigned int)Ncb;
540 
541  for(unsigned int t=0;t<Ncb_;t++)
542  {
543  //if(Pt[t]!=0)
544  if(Pt[t]>seuilevitinf)
545  {
546  for(unsigned int r=0;r<Ncb_;r++)
547  {
548  //if(Prt[r*Ncb+t]!=0)
549  if(Prt[r*Ncb_+t]>seuilevitinf)
550  {
551  for(unsigned int it=0;it<nbParam;it++){
552  // dPxy/dt
553  dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
554  }
555  //dtemp=1.+log(Prt[r*Ncb+t]/Pt[t]);
556  // u = som(Pxy.logPxPy)
557  u += Prt[r*Ncb_+t]*log(Pt[t]*Pr[r]);
558  // v = som(Pxy.logPxy)
559  v += Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
560 
561  for(unsigned int it=0;it<nbParam;it++){
562  // u' = som dPxylog(PxPy)
563  du[it] += dprtemp[it]*log(Pt[t]*Pr[r]);
564  // v' = som dPxy(1+log(Pxy))
565  dv[it] += dprtemp[it]*(1+log(Prt[r*Ncb_+t]));
566 
567  }
568  for(unsigned int it=0;it<nbParam;it++){
569  for(unsigned int jt=0;jt<nbParam;jt++){
570  d2u[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*log(Pt[t]*Pr[r])
571  + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
572  d2v[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*(1+log(Prt[r*Ncb_+t]))
573  + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
574  }
575  }
576  }
577  }
578  }
579  }
580  // B = v2
581  B = (v*v);
582  for(unsigned int it=0;it<nbParam;it++){
583  // A = u'v-uv'
584  A[it] = du[it] * v - u * dv[it];
585  // B' = 2vv'
586  dB[it] = 2 * v * dv[it];
587  // G = (u'v-v'u)/v2
588  // G[it] = A[it]/B;
589  for(unsigned int jt=0;jt<nbParam;jt++){
590  // A' = u''v-v''u
591  dA[it][jt] = d2u[it][jt]*v-d2v[it][jt]*u;
592  // Hessian = (A'B-AB')/B2
593  Hessian[it][jt] = (dA[it][jt] * B -A[it] * dB[it])/(B*B);
594  }
595  }
596 
597  delete[] du;
598  delete[] dv;
599  delete[] A;
600  delete[] dB;
601  for (unsigned int i = 0; i < nbParam; i++) {
602  delete[] d2u[i];
603  delete[] d2v[i];
604  delete[] dA[i];
605  }
606  delete[] d2u;
607  delete[] d2v;
608  delete[] dA;
609 
610  // std::cout<<"Hdes - compute Hessien\n"<<u<<"\n"<<v<<"\n"<<du[0]<<" "<<du[1]<<" "<<du[2]<<"\n"<<dv[2]<<"\n"<<d2u[2][2]<<"\n"<<d2v[2][2]<<"\n"<<H<<std::endl;
611 }
612 
613 
614 void vpTemplateTrackerMI::computeGradient()
615 {
616 
617  double seuilevitinf=1e-200;
618  G=0;
619  unsigned int Ncb_ = (unsigned int)Ncb;
620  double dtemp;
621  for(unsigned int t=0;t<Ncb_;t++)
622  {
623  //if(Pt[t]!=0)
624  if(Pt[t]>seuilevitinf)
625  {
626  for(unsigned int r=0;r<Ncb_;r++)
627  {
628  if(Prt[r*Ncb_+t]>seuilevitinf)
629  //if(Prt[r*Ncb+t]!=0)
630  {
631  for(unsigned int it=0;it<nbParam;it++)
632  dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
633 
634  dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
635 
636  for(unsigned int it=0;it<nbParam;it++)
637  G[it]+=dtemp*dprtemp[it];
638  }
639  }
640  }
641  }
642 
643 }
644 void vpTemplateTrackerMI::zeroProbabilities()
645 {
646  unsigned int Ncb_ = (unsigned int)Ncb;
647  unsigned int Nc_ = (unsigned int)Nc;
648  unsigned int influBspline_ = (unsigned int)influBspline;
649 
650  memset(Prt, 0, Ncb_*Ncb_*sizeof(double));
651  memset(dPrt, 0, Ncb_*Ncb_*nbParam*sizeof(double));
652  memset(d2Prt, 0, Ncb_*Ncb_*nbParam*nbParam*sizeof(double));
653  memset(PrtTout, 0, Nc_*Nc_*influBspline_*(1+nbParam+nbParam*nbParam)*sizeof(double));
654 
655  // std::cout << Ncb*Ncb << std::endl;
656  // std::cout << Ncb*Ncb*nbParam << std::endl;
657  // std::cout << Ncb*Ncb*nbParam*nbParam << std::endl;
658  // std::cout << Ncb*Ncb*influBspline*(1+nbParam+nbParam*nbParam) << std::endl;
659 }
660 
661 double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I,int &nc, const int &bspline_,vpColVector &tp)
662 {
663  unsigned int tNcb = (unsigned int)(nc+bspline_);
664  unsigned int tinfluBspline = (unsigned int)(bspline_*bspline_);
665  unsigned int nc_ = (unsigned int)nc;
666  double *tPrtD = new double[nc_*nc_*tinfluBspline];
667  double *tPrt = new double[tNcb*tNcb];
668  double *tPr = new double[tNcb];
669  double *tPt = new double[tNcb];
670 
671  double MI=0;
672  int Nbpoint=0;
673  double IW;
674 
675  vpImage<double> GaussI ;
676  vpImageFilter::filter(I, GaussI,fgG,taillef);
677 
678  memset(tPrt, 0, tNcb*tNcb*sizeof(double));
679  memset(tPrtD, 0, nc_*nc_*tinfluBspline*sizeof(double));
680 
681  //Warp->ComputeMAtWarp(tp);
682  Warp->computeCoeff(tp);
683  for(unsigned int point=0;point<templateSize;point++)
684  {
685  int i=ptTemplate[point].y;
686  int j=ptTemplate[point].x;
687  X1[0]=j;X1[1]=i;
688 
689  Warp->computeDenom(X1,tp);
690  Warp->warpX(X1,X2,tp);
691  double j2=X2[0];
692  double i2=X2[1];
693 
694  //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
695  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth())-1)
696  {
697  Nbpoint++;
698 
699  double Tij=ptTemplate[point].val;
700  if(!blur)
701  IW=I.getValue(i2,j2);
702  else
703  IW=GaussI.getValue(i2,j2);
704 
705  int cr=(int)((IW*(nc-1))/255.);
706  int ct=(int)((Tij*(nc-1))/255.);
707  double er=(IW*(nc-1))/255.-cr;
708  double et=((double)Tij*(nc-1))/255.-ct;
709 
710  //Calcul de l'histogramme joint par interpolation bilineaire (Bspline_ ordre 1)
711  vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1.,bspline_);
712  }
713  }
714  double *pt=tPrtD;
715  int tNcb_ = (int)tNcb;
716  int tinfluBspline_ = (int)tinfluBspline;
717  for(int r=0;r<nc;r++)
718  for(int t=0;t<nc;t++)
719  {
720  for(int i=0;i<tinfluBspline_;i++)
721  {
722  int r2,t2;
723  r2=r+i/bspline_;
724  t2=t+i%bspline_;
725  tPrt[r2*tNcb_+t2]+=*pt;
726 
727  pt++;
728  }
729  }
730 
731  if (Nbpoint == 0) {
732  delete[] tPrtD;
733  delete[] tPrt;
734  delete[] tPr;
735  delete[] tPt;
736 
737  return 0;
738  }
739  else
740  {
741  for(unsigned int r=0;r<tNcb;r++)
742  for(unsigned int t=0;t<tNcb;t++)
743  //printf("%f ",tPrt[r*tNcb+t]);
744  tPrt[r*tNcb+t]=tPrt[r*tNcb+t]/Nbpoint;
745  //calcul Pr;
746  memset(tPr, 0, tNcb*sizeof(double));
747  for(unsigned int r=0;r<tNcb;r++)
748  {
749  for(unsigned int t=0;t<tNcb;t++)
750  tPr[r]+=tPrt[r*tNcb+t];
751  }
752 
753  //calcul Pt;
754  memset(tPt, 0, (size_t)(tNcb*sizeof(double)));
755  for(unsigned int t=0;t<tNcb;t++)
756  {
757  for(unsigned int r=0;r<tNcb;r++)
758  tPt[t]+=tPrt[r*tNcb+t];
759  }
760  for(unsigned int r=0;r<tNcb;r++)
761  //if(tPr[r]!=0)
762  if(std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
763  MI-=tPr[r]*log(tPr[r]);
764 
765  for(unsigned int t=0;t<tNcb;t++)
766  //if(tPt[t]!=0)
767  if(std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
768  MI-=tPt[t]*log(tPt[t]);
769 
770  for(unsigned int r=0;r<tNcb;r++)
771  for(unsigned int t=0;t<tNcb;t++)
772  //if(tPrt[r*tNcb+t]!=0)
773  if(std::fabs(tPrt[r*tNcb+t]) > std::numeric_limits<double>::epsilon())
774  MI+=tPrt[r*tNcb+t]*log(tPrt[r*tNcb+t]);
775  }
776  delete[] tPrtD;
777  delete[] tPrt;
778  delete[] tPr;
779  delete[] tPt;
780 
781  return MI;
782 }
783 
784 double vpTemplateTrackerMI::getMI256(const vpImage<unsigned char> &I, const vpColVector &tp)
785 {
786  vpMatrix Prt256(256,256);Prt256=0;
787  vpColVector Pr256(256);Pr256=0;
788  vpColVector Pt256(256);Pt256=0;
789 
790  int Nbpoint=0;
791  unsigned int Tij,IW;
792 
793  vpImage<double> GaussI ;
794  if(blur)
795  vpImageFilter::filter(I, GaussI,fgG,taillef);
796 
797  //Warp->ComputeMAtWarp(tp);
798  Warp->computeCoeff(tp);
799  for(unsigned int point=0;point<templateSize;point++)
800  {
801  int i=ptTemplate[point].y;
802  int j=ptTemplate[point].x;
803  X1[0]=j;X1[1]=i;
804 
805  Warp->computeDenom(X1,tp);
806  Warp->warpX(X1,X2,tp);
807  double j2=X2[0];
808  double i2=X2[1];
809 
810  //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
811  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth())-1)
812  {
813  Nbpoint++;
814 
815  Tij=(unsigned int)ptTemplate[point].val;
816  if(!blur)
817  IW=(unsigned int)I.getValue(i2,j2);
818  else
819  IW=(unsigned int)GaussI.getValue(i2,j2);
820 
821  Prt256[Tij][IW]++;
822  Pr256[Tij]++;
823  Pt256[IW]++;
824  }
825  }
826 
827  if (Nbpoint == 0){
828  throw(vpException(vpException::divideByZeroError,
829  "Cannot get MI; number of points = 0"));
830  }
831  Prt256=Prt256/Nbpoint;
832  Pr256=Pr256/Nbpoint;
833  Pt256=Pt256/Nbpoint;
834 
835  double MI=0;
836 
837  for(unsigned int t=0;t<256;t++)
838  {
839  for(unsigned int r=0;r<256;r++)
840  {
841  //if(Prt256[r][t]!=0)
842  if(std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
843  MI+=Prt256[r][t]*log(Prt256[r][t]);
844  }
845  //if(Pt256[t]!=0)
846  if(std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
847  MI+=-Pt256[t]*log(Pt256[t]);
848  //if(Pr256[t]!=0)
849  if(std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
850  MI+=-Pr256[t]*log(Pr256[t]);
851 
852  }
853  return MI;
854 }
vpTemplateTrackerMI::vpTemplateTrackerMI
vpTemplateTrackerMI()
Default constructor.
Definition: vpTemplateTrackerMI.h:148
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
vpTemplateTrackerMI::computeHessien
void computeHessien(vpMatrix &H)
Definition: vpTemplateTrackerMI.cpp:468
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:226
vpArray2D::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
vpTemplateTracker::ptTemplate
vpTemplateTrackerPoint * ptTemplate
Definition: vpTemplateTracker.h:68
vpTemplateTrackerWarp::warpX
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpImage::getValue
Type getValue(double i, double j) const
Definition: vpImage.h:1477
vpTemplateTracker::BI
vpImage< double > BI
Definition: vpTemplateTracker.h:134
vpTemplateTrackerMI::ApproxHessian
vpHessienApproximationType ApproxHessian
Definition: vpTemplateTrackerMI.h:80
vpTemplateTrackerMI::getMI256
double getMI256(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:784
vpTemplateTracker::templateSize
unsigned int templateSize
Definition: vpTemplateTracker.h:71
vpTrackingException
Error that can be emited by the vpTracker class and its derivates.
Definition: vpTrackingException.h:65
vpTemplateTrackerMI::getCost
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:134
vpTemplateTrackerMI::computeMI
void computeMI(double &MI)
Definition: vpTemplateTrackerMI.cpp:418
vpTemplateTrackerMI::getNormalizedCost
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:238
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:175
vpImageFilter::filter
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
Definition: vpImageFilter.cpp:128
vpTemplateTrackerMI::computeHessienNormalized
void computeHessienNormalized(vpMatrix &H)
Definition: vpTemplateTrackerMI.cpp:507
vpTemplateTrackerMI::setBspline
void setBspline(const vpBsplineType &newbs)
Definition: vpTemplateTrackerMI.cpp:43
vpTemplateTracker::Warp
vpTemplateTrackerWarp * Warp
Definition: vpTemplateTracker.h:123
vpTemplateTrackerMI::computeProba
void computeProba(int &nbpoint)
Definition: vpTemplateTrackerMI.cpp:328
vpTemplateTrackerMI::hessianComputation
vpHessienType hessianComputation
Definition: vpTemplateTrackerMI.h:79
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225