Visual Servoing Platform  version 3.0.0
vpTemplateTrackerMI.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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 i2,j2;
139  double Tij;
140  double IW;
141 
142  int i,j;
143  int cr,ct;
144  double er,et;
145  unsigned int Ncb_ = (unsigned int) Ncb;
146 
147  memset(Prt, 0, Ncb*Ncb*sizeof(double));
148  memset(PrtD, 0, Nc*Nc*influBspline*sizeof(double));
149 
150  //Warp->ComputeMAtWarp(tp);
151  Warp->computeCoeff(tp);
152  for(unsigned int point=0;point<templateSize;point++)
153  {
154  i=ptTemplate[point].y;
155  j=ptTemplate[point].x;
156  X1[0]=j;X1[1]=i;
157 
158  Warp->computeDenom(X1,tp);
159  Warp->warpX(X1,X2,tp);
160  j2=X2[0];i2=X2[1];
161 
162  //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
163  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
164  {
165  Nbpoint++;
166 
167  Tij=ptTemplate[point].val;
168  if(!blur)
169  IW=I.getValue(i2,j2);
170  else
171  IW=BI.getValue(i2,j2);
172 
173  cr=(int)((IW*(Nc-1))/255.);
174  ct=(int)((Tij*(Nc-1))/255.);
175  er=(IW*(Nc-1))/255.-cr;
176  et=((double)Tij*(Nc-1))/255.-ct;
177 
178  //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
179  vpTemplateTrackerMIBSpline::PutPVBsplineD(PrtD, cr, er, ct, et, Nc, 1.,bspline);
180  }
181  }
182 
183  ratioPixelIn=(double)Nbpoint/(double)templateSize;
184 
185  double *pt=PrtD;
186  for(int r=0;r<Nc;r++)
187  for(int t=0;t<Nc;t++)
188  {
189  for(i=0;i<influBspline;i++)
190  {
191  int r2,t2;
192  r2=r+i/bspline;
193  t2=t+i%bspline;
194  Prt[r2*Ncb+t2]+=*pt;
195 
196  pt++;
197  }
198  }
199 
200  if(Nbpoint==0)
201  return 0;
202  for(unsigned int r=0;r<Ncb_;r++)
203  for(unsigned int t=0;t<Ncb_;t++)
204  //printf("%f ",Prt[r*Ncb+t]);
205  Prt[r*Ncb_+t]=Prt[r*Ncb_+t]/Nbpoint;
206  //calcul Pr;
207  memset(Pr, 0, Ncb_*sizeof(double));
208  for(unsigned int r=0;r<Ncb_;r++)
209  {
210  for(unsigned int t=0;t<Ncb_;t++)
211  Pr[r]+=Prt[r*Ncb_+t];
212  }
213 
214  //calcul Pt;
215  memset(Pt, 0, Ncb_*sizeof(double));
216  for(unsigned int t=0;t<Ncb_;t++)
217  {
218  for(unsigned int r=0;r<Ncb_;r++)
219  Pt[t]+=Prt[r*Ncb_+t];
220  }
221  for(unsigned int r=0;r<Ncb_;r++)
222  //if(Pr[r]!=0)
223  if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
224  MI-=Pr[r]*log(Pr[r]);
225 
226  for(unsigned int t=0;t<Ncb_;t++)
227  //if(Pt[t]!=0)
228  if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
229  MI-=Pt[t]*log(Pt[t]);
230 
231  for(unsigned int r=0;r<Ncb_;r++)
232  for(unsigned int t=0;t<Ncb_;t++)
233  //if(Prt[r*Ncb+t]!=0)
234  if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
235  MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
236 
237  return -MI;
238 }
239 
240 double vpTemplateTrackerMI::getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp)
241 {
242  // Attention, cette version calculée de la NMI ne pourra pas atteindre le maximum de 2
243  // Ceci est du au fait que par defaut, l'image est floutée dans vpTemplateTracker::initTracking()
244 
245  double MI=0;
246  double Nbpoint=0;
247  double i2,j2;
248  double Tij;
249  double IW;
250 
251  double Pr_[256];
252  double Pt_[256];
253  double Prt_[256][256];
254 
255  memset(Pr_, 0, 256*sizeof(double));
256  memset(Pt_, 0, 256*sizeof(double));
257  memset(Prt_, 0, 256*256*sizeof(double));
258 
259  for(unsigned int point=0;point<templateSize;point++)
260  {
261  int i=ptTemplate[point].y;
262  int j=ptTemplate[point].x;
263  X1[0]=j;X1[1]=i;
264 
265  Warp->computeDenom(X1,tp);
266  Warp->warpX(X1,X2,tp);
267  j2=X2[0];i2=X2[1];
268 
269  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
270  {
271  Nbpoint++;
272  Tij=ptTemplate[point].val;
273  if(!blur)
274  IW=I[(int)i2][(int)j2];
275  else
276  IW=BI.getValue(i2,j2);
277 
278  Pr_[(int)Tij]++;
279  Pt_[(int)IW]++;
280  Prt_[(int)Tij][(int)IW]++;
281  }
282  }
283 
284  for(int i = 0 ; i < 256 ; i++)
285  {
286  Pr_[i] /= Nbpoint;
287  Pt_[i] /= Nbpoint;
288  for(int j = 0 ; j < 256 ; j++)
289  Prt_[i][j] /= Nbpoint;
290  }
291 
292  for(int r=0;r<256;r++)
293  //if(Pr_[r]!=0)
294  if(std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon())
295  MI-=Pr_[r]*log(Pr_[r]);
296 
297  for(int t=0;t<256;t++)
298  //if(Pt_[t]!=0)
299  if(std::fabs(Pt_[t]) > std::numeric_limits<double>::epsilon())
300  MI-=Pt_[t]*log(Pt_[t]);
301 
302  double denom = 0;
303  for(int r=0;r<256;r++)
304  for(int t=0;t<256;t++)
305  //if(Prt_[r][t]!=0)
306  if(std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon())
307  denom-=(Prt_[r][t]*log(Prt_[r][t]));
308 
309  //if(denom != 0)
310  if(std::fabs(denom) > std::numeric_limits<double>::epsilon())
311  MI = MI/denom;
312  else MI = 0;
313 
314  return -MI;
315 }
316 
317 vpTemplateTrackerMI::~vpTemplateTrackerMI()
318 {
319  if (Pt) delete[] Pt;
320  if (Pr) delete[] Pr;
321  if (Prt) delete[] Prt;
322  if (dPrt) delete[] dPrt;
323  if (d2Prt) delete[] d2Prt;
324  if (PrtD) delete[] PrtD;
325  if (dPrtD) delete[] dPrtD;
326  if (PrtTout) delete[] PrtTout;
327  if (temp) delete[] temp;
328  if (dprtemp) delete[] dprtemp;
329 }
330 
331 void vpTemplateTrackerMI::computeProba(int &nbpoint)
332 {
333  double *pt=PrtTout;
334  unsigned int Nc_ = (unsigned int)Nc;
335  unsigned int Ncb_ = (unsigned int)Ncb;
336  unsigned int bspline_ = (unsigned int)bspline;
337 
338  for(unsigned int r=0;r<Nc_;r++) {
339  for(unsigned int t=0;t<Nc_;t++)
340  {
341  for(unsigned int r2=0;r2<bspline_;r2++) {
342  for(unsigned int t2=0;t2<bspline_;t2++)
343  {
344  Prt[((r2+r)*Ncb_+(t2+t))]+=*pt++;
345  for(unsigned int ip=0;ip<nbParam;ip++)
346  {
347  dPrt[((r2+r)*Ncb_+(t2+t))*nbParam+ip]+=*pt++;
348  for(unsigned int it=0;it<nbParam;it++){
349  d2Prt[((r2+r)*Ncb_+(t2+t))*nbParam*nbParam+ip*nbParam+it]+=*pt++;
350  }
351  }
352  }
353  }
354  }
355  }
356 
357  // for(unsigned int r=0;r<Nc;r++)
358  // for(unsigned int t=0;t<Nc;t++)
359  // {
360  // for(int r2=0;r2<bspline;r2++)
361  // for(int t2=0;t2<bspline;t2++)
362  // {
363  // Prt[((r2+r)*Ncb+(t2+t))]+=*pt++;
364  // for(int ip=0;ip<nbParam;ip++)
365  // {
366  // dPrt[((r2+r)*Ncb+(t2+t))*nbParam+ip]+=*pt++;
367  // }
368  // for(int ip=0;ip<nbParam;ip++)
369  // {
370  // for(int it=0;it<nbParam;it++)
371  // d2Prt[((r2+r)*Ncb+(t2+t))*nbParam*nbParam+ip*nbParam+it]+=*pt++;
372  // }
373  // }
374  // }
375 
376  // int vr2, vt2, vt2ip;
377  // for(unsigned int r=0;r<Nc;r++)
378  // for(unsigned int t=0;t<Nc;t++)
379  // {
380  // for(int r2=0;r2<bspline;r2++)
381  // {
382  // vr2 = (r2+r)*Ncb;
383  // for(int t2=0;t2<bspline;t2++)
384  // {
385  // vt2 = vr2+(t2+t);
386  // Prt[vt2]+=*pt++;
387  // vt2 *= nbParam;
388 
389  // for(int ip=0;ip<nbParam;ip++)
390  // dPrt[vt2+ip]+=*pt++;
391 
392  // for(int ip=0;ip<nbParam;ip++)
393  // {
394  // vt2ip = vt2*nbParam+ip*nbParam;
395  // for(int it=0;it<nbParam;it++)
396  // d2Prt[vt2ip+it]+=*pt++;
397  // }
398  // }
399  // }
400  // }
401 
402  if(nbpoint==0) {
403  //std::cout<<"plus de point dans template suivi"<<std::endl;
404  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
405  }
406  unsigned int indd, indd2;
407  indd = indd2 = 0;
408  for(int i=0;i<Ncb*Ncb;i++){
409  Prt[i]=Prt[i]/nbpoint;
410  for(unsigned int j=0;j<nbParam;j++){
411  dPrt[indd]=dPrt[indd]/nbpoint;
412  indd++;
413  for(unsigned int k=0;k<nbParam;k++){
414  d2Prt[indd2]=d2Prt[indd2]/nbpoint;
415  indd2++;
416  }
417  }
418  }
419 }
420 
421 void vpTemplateTrackerMI::computeMI(double &MI)
422 {
423  unsigned int Ncb_ = (unsigned int) Ncb;
424 
425  //calcul Pr;
426  memset(Pr, 0, Ncb_*sizeof(double));
427  for(unsigned int r=0;r<Ncb_;r++)
428  {
429  for(unsigned int t=0;t<Ncb_;t++)
430  Pr[r]+=Prt[r*Ncb_+t];
431  }
432 
433  //calcul Pt;
434  memset(Pt, 0, Ncb_*sizeof(double));
435  for(unsigned int t=0;t<Ncb_;t++)
436  {
437  for(unsigned int r=0;r<Ncb_;r++)
438  Pt[t]+=Prt[r*Ncb_+t];
439  }
440 
441  //calcul Entropies;
442  double entropieI=0;
443  for(unsigned int r=0;r<Ncb_;r++)
444  {
445  //if(Pr[r]!=0)
446  if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
447  {
448  entropieI-=Pr[r]*log(Pr[r]);
449  MI-=Pr[r]*log(Pr[r]);
450  }
451  }
452 
453  double entropieT=0;
454  for(unsigned int t=0;t<Ncb_;t++)
455  {
456  //if(Pt[t]!=0)
457  if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
458  {
459  entropieT-=Pt[t]*log(Pt[t]);
460  MI-=Pt[t]*log(Pt[t]);
461  }
462  }
463 
464  for(unsigned int r=0;r<Ncb_;r++)
465  for(unsigned int t=0;t<Ncb_;t++)
466  //if(Prt[r*Ncb+t]!=0)
467  if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
468  MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
469 }
470 
471 void vpTemplateTrackerMI::computeHessien(vpMatrix &Hessian)
472 {
473  double seuilevitinf=1e-200;
474  Hessian=0;
475  double dtemp;
476  unsigned int Ncb_ = (unsigned int)Ncb;
477  for(unsigned int t=0;t<Ncb_;t++)
478  {
479  //if(Pt[t]!=0)
480  if(Pt[t]>seuilevitinf)
481  {
482  for(unsigned int r=0;r<Ncb_;r++)
483  {
484  //if(Prt[r*Ncb+t]!=0)
485  if(Prt[r*Ncb_+t]>seuilevitinf)
486  {
487  for(unsigned int it=0;it<nbParam;it++)
488  dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
489 
490  dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
491  for(unsigned int it=0;it<nbParam;it++)
492  for(unsigned int jt=0;jt<nbParam;jt++)
493  if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_NEW)
494  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;
495  else if(ApproxHessian==HESSIAN_NEW)
496  Hessian[it][jt]+=d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*dtemp;
497  else
498  Hessian[it][jt]+=dprtemp[it]*dprtemp[jt]*(1./Prt[r*Ncb_+t]-1./Pt[t]);
499  /*std::cout<<"Prt[r*Ncb+t]="<<Prt[r*Ncb+t]<<std::endl;
500  std::cout<<"Pt[t]="<<Pt[t]<<std::endl;
501 
502  std::cout<<"Hessian="<<Hessian<<std::endl;*/
503  }
504  }
505  }
506  }
507  // std::cout<<"Hessian="<<Hessian<<std::endl;
508 }
509 
510 void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
511 {
512  double seuilevitinf=1e-200;
513  //double dtemp;
514  double u=0,v=0,B=0;
515  double *du = new double[nbParam];
516  double *dv = new double[nbParam];
517  double *A = new double[nbParam];
518  double *dB = new double[nbParam];
519  double **d2u = new double *[nbParam];
520  double **d2v = new double *[nbParam];
521  double **dA = new double *[nbParam];
522  for (unsigned int i = 0; i < nbParam; i++) {
523  d2u[i] = new double[nbParam];
524  d2v[i] = new double[nbParam];
525  dA[i] = new double[nbParam];
526  }
527 
528  memset(du, 0, nbParam*sizeof(double));
529  memset(dv, 0, nbParam*sizeof(double));
530  memset(A, 0, nbParam*sizeof(double));
531  memset(dB, 0, nbParam*sizeof(double));
532  memset(dprtemp, 0, nbParam*sizeof(double));
533 
534  for(unsigned int it=0;it<nbParam;it++){
535  for(unsigned int jt=0;jt<nbParam;jt++){
536  memset(d2u[it], 0, nbParam*sizeof(double));
537  memset(d2v[it], 0, nbParam*sizeof(double));
538  memset(dA[it], 0, nbParam*sizeof(double));
539  }
540  }
541 
542  unsigned int Ncb_ = (unsigned int)Ncb;
543 
544  for(unsigned int t=0;t<Ncb_;t++)
545  {
546  //if(Pt[t]!=0)
547  if(Pt[t]>seuilevitinf)
548  {
549  for(unsigned int r=0;r<Ncb_;r++)
550  {
551  //if(Prt[r*Ncb+t]!=0)
552  if(Prt[r*Ncb_+t]>seuilevitinf)
553  {
554  for(unsigned int it=0;it<nbParam;it++){
555  // dPxy/dt
556  dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
557  }
558  //dtemp=1.+log(Prt[r*Ncb+t]/Pt[t]);
559  // u = som(Pxy.logPxPy)
560  u += Prt[r*Ncb_+t]*log(Pt[t]*Pr[r]);
561  // v = som(Pxy.logPxy)
562  v += Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
563 
564  for(unsigned int it=0;it<nbParam;it++){
565  // u' = som dPxylog(PxPy)
566  du[it] += dprtemp[it]*log(Pt[t]*Pr[r]);
567  // v' = som dPxy(1+log(Pxy))
568  dv[it] += dprtemp[it]*(1+log(Prt[r*Ncb_+t]));
569 
570  }
571  for(unsigned int it=0;it<nbParam;it++){
572  for(unsigned int jt=0;jt<nbParam;jt++){
573  d2u[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*log(Pt[t]*Pr[r])
574  + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
575  d2v[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*(1+log(Prt[r*Ncb_+t]))
576  + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
577  }
578  }
579  }
580  }
581  }
582  }
583  // B = v2
584  B = (v*v);
585  for(unsigned int it=0;it<nbParam;it++){
586  // A = u'v-uv'
587  A[it] = du[it] * v - u * dv[it];
588  // B' = 2vv'
589  dB[it] = 2 * v * dv[it];
590  // G = (u'v-v'u)/v2
591  // G[it] = A[it]/B;
592  for(unsigned int jt=0;jt<nbParam;jt++){
593  // A' = u''v-v''u
594  dA[it][jt] = d2u[it][jt]*v-d2v[it][jt]*u;
595  // Hessian = (A'B-AB')/B2
596  Hessian[it][jt] = (dA[it][jt] * B -A[it] * dB[it])/(B*B);
597  }
598  }
599 
600  delete[] du;
601  delete[] dv;
602  delete[] A;
603  delete[] dB;
604  for (unsigned int i = 0; i < nbParam; i++) {
605  delete[] d2u[i];
606  delete[] d2v[i];
607  delete[] dA[i];
608  }
609  delete[] d2u;
610  delete[] d2v;
611  delete[] dA;
612 
613  // 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;
614 }
615 
616 
617 void vpTemplateTrackerMI::computeGradient()
618 {
619 
620  double seuilevitinf=1e-200;
621  G=0;
622  unsigned int Ncb_ = (unsigned int)Ncb;
623  double dtemp;
624  for(unsigned int t=0;t<Ncb_;t++)
625  {
626  //if(Pt[t]!=0)
627  if(Pt[t]>seuilevitinf)
628  {
629  for(unsigned int r=0;r<Ncb_;r++)
630  {
631  if(Prt[r*Ncb_+t]>seuilevitinf)
632  //if(Prt[r*Ncb+t]!=0)
633  {
634  for(unsigned int it=0;it<nbParam;it++)
635  dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
636 
637  dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
638 
639  for(unsigned int it=0;it<nbParam;it++)
640  G[it]+=dtemp*dprtemp[it];
641  }
642  }
643  }
644  }
645 
646 }
647 void vpTemplateTrackerMI::zeroProbabilities()
648 {
649  unsigned int Ncb_ = (unsigned int)Ncb;
650  unsigned int Nc_ = (unsigned int)Nc;
651  unsigned int influBspline_ = (unsigned int)influBspline;
652 
653  memset(Prt, 0, Ncb_*Ncb_*sizeof(double));
654  memset(dPrt, 0, Ncb_*Ncb_*nbParam*sizeof(double));
655  memset(d2Prt, 0, Ncb_*Ncb_*nbParam*nbParam*sizeof(double));
656  memset(PrtTout, 0, Nc_*Nc_*influBspline_*(1+nbParam+nbParam*nbParam)*sizeof(double));
657 
658  // std::cout << Ncb*Ncb << std::endl;
659  // std::cout << Ncb*Ncb*nbParam << std::endl;
660  // std::cout << Ncb*Ncb*nbParam*nbParam << std::endl;
661  // std::cout << Ncb*Ncb*influBspline*(1+nbParam+nbParam*nbParam) << std::endl;
662 }
663 
664 double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I,int &nc, const int &bspline_,vpColVector &tp)
665 {
666  int tNcb=nc+bspline_;
667  int tinfluBspline=bspline_*bspline_;
668  double *tPrtD = new double[nc*nc*tinfluBspline];
669  double *tPrt = new double[tNcb*tNcb];
670  double *tPr = new double[tNcb];
671  double *tPt = new double[tNcb];
672 
673  double MI=0;
674  int Nbpoint=0;
675  double i2,j2;
676  double Tij;
677  double IW;
678 
679  vpImage<double> GaussI ;
680  vpImageFilter::filter(I, GaussI,fgG,taillef);
681 
682  int i,j;
683  int cr,ct;
684  double er,et;
685  memset(tPrt, 0, tNcb*tNcb*sizeof(double));
686  memset(tPrtD, 0, nc*nc*tinfluBspline*sizeof(double));
687 
688  //Warp->ComputeMAtWarp(tp);
689  Warp->computeCoeff(tp);
690  for(unsigned int point=0;point<templateSize;point++)
691  {
692  i=ptTemplate[point].y;
693  j=ptTemplate[point].x;
694  X1[0]=j;X1[1]=i;
695 
696  Warp->computeDenom(X1,tp);
697  Warp->warpX(X1,X2,tp);
698  j2=X2[0];i2=X2[1];
699 
700  //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
701  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth())-1)
702  {
703  Nbpoint++;
704 
705  Tij=ptTemplate[point].val;
706  if(!blur)
707  IW=I.getValue(i2,j2);
708  else
709  IW=GaussI.getValue(i2,j2);
710 
711  cr=(int)((IW*(nc-1))/255.);
712  ct=(int)((Tij*(nc-1))/255.);
713  er=(IW*(nc-1))/255.-cr;
714  et=((double)Tij*(nc-1))/255.-ct;
715 
716  //Calcul de l'histogramme joint par interpolation bilineaire (Bspline_ ordre 1)
717  vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1.,bspline_);
718  }
719  }
720  double *pt=tPrtD;
721  for(int r=0;r<nc;r++)
722  for(int t=0;t<nc;t++)
723  {
724  for(i=0;i<tinfluBspline;i++)
725  {
726  int r2,t2;
727  r2=r+i/bspline_;
728  t2=t+i%bspline_;
729  tPrt[r2*tNcb+t2]+=*pt;
730 
731  pt++;
732  }
733  }
734 
735  if (Nbpoint == 0) {
736  delete[] tPrtD;
737  delete[] tPrt;
738  delete[] tPr;
739  delete[] tPt;
740 
741  return 0;
742  }
743  else
744  {
745  for(int r=0;r<tNcb;r++)
746  for(int t=0;t<tNcb;t++)
747  //printf("%f ",tPrt[r*tNcb+t]);
748  tPrt[r*tNcb+t]=tPrt[r*tNcb+t]/Nbpoint;
749  //calcul Pr;
750  memset(tPr, 0, tNcb*sizeof(double));
751  for(int r=0;r<tNcb;r++)
752  {
753  for(int t=0;t<tNcb;t++)
754  tPr[r]+=tPrt[r*tNcb+t];
755  }
756 
757  //calcul Pt;
758  memset(tPt, 0, tNcb*sizeof(double));
759  for(int t=0;t<tNcb;t++)
760  {
761  for(int r=0;r<tNcb;r++)
762  tPt[t]+=tPrt[r*tNcb+t];
763  }
764  for(int r=0;r<tNcb;r++)
765  //if(tPr[r]!=0)
766  if(std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
767  MI-=tPr[r]*log(tPr[r]);
768 
769  for(int t=0;t<tNcb;t++)
770  //if(tPt[t]!=0)
771  if(std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
772  MI-=tPt[t]*log(tPt[t]);
773 
774  for(int r=0;r<tNcb;r++)
775  for(int t=0;t<tNcb;t++)
776  //if(tPrt[r*tNcb+t]!=0)
777  if(std::fabs(tPrt[r*tNcb+t]) > std::numeric_limits<double>::epsilon())
778  MI+=tPrt[r*tNcb+t]*log(tPrt[r*tNcb+t]);
779  }
780  delete[] tPrtD;
781  delete[] tPrt;
782  delete[] tPr;
783  delete[] tPt;
784 
785  return MI;
786 }
787 
788 double vpTemplateTrackerMI::getMI256(const vpImage<unsigned char> &I, const vpColVector &tp)
789 {
790  vpMatrix Prt256(256,256);Prt256=0;
791  vpColVector Pr256(256);Pr256=0;
792  vpColVector Pt256(256);Pt256=0;
793 
794  int Nbpoint=0;
795  double i2,j2;
796  unsigned int Tij,IW;
797 
798  int i,j;
799  vpImage<double> GaussI ;
800  if(blur)
801  vpImageFilter::filter(I, GaussI,fgG,taillef);
802 
803  //Warp->ComputeMAtWarp(tp);
804  Warp->computeCoeff(tp);
805  for(unsigned int point=0;point<templateSize;point++)
806  {
807  i=ptTemplate[point].y;
808  j=ptTemplate[point].x;
809  X1[0]=j;X1[1]=i;
810 
811  Warp->computeDenom(X1,tp);
812  Warp->warpX(X1,X2,tp);
813  j2=X2[0];i2=X2[1];
814 
815  //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
816  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth())-1)
817  {
818  Nbpoint++;
819 
820  Tij=(unsigned int)ptTemplate[point].val;
821  if(!blur)
822  IW=(unsigned int)I.getValue(i2,j2);
823  else
824  IW=(unsigned int)GaussI.getValue(i2,j2);
825 
826  Prt256[Tij][IW]++;
827  Pr256[Tij]++;
828  Pt256[IW]++;
829  }
830  }
831 
832  if (Nbpoint == 0){
833  throw(vpException(vpException::divideByZeroError,
834  "Cannot get MI; number of points = 0"));
835  }
836  Prt256=Prt256/Nbpoint;
837  Pr256=Pr256/Nbpoint;
838  Pt256=Pt256/Nbpoint;
839 
840  double MI=0;
841 
842  for(unsigned int t=0;t<256;t++)
843  {
844  for(unsigned int r=0;r<256;r++)
845  {
846  //if(Prt256[r][t]!=0)
847  if(std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
848  MI+=Prt256[r][t]*log(Prt256[r][t]);
849  }
850  //if(Pt256[t]!=0)
851  if(std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
852  MI+=-Pt256[t]*log(Pt256[t]);
853  //if(Pr256[t]!=0)
854  if(std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
855  MI+=-Pr256[t]*log(Pr256[t]);
856 
857  }
858  return MI;
859 }
vpTemplateTrackerMI::vpTemplateTrackerMI
vpTemplateTrackerMI()
Default constructor.
Definition: vpTemplateTrackerMI.h:148
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
vpTemplateTrackerMI::computeHessien
void computeHessien(vpMatrix &H)
Definition: vpTemplateTrackerMI.cpp:471
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:161
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:66
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:1148
vpTemplateTracker::BI
vpImage< double > BI
Definition: vpTemplateTracker.h:132
vpTemplateTrackerMI::ApproxHessian
vpHessienApproximationType ApproxHessian
Definition: vpTemplateTrackerMI.h:80
vpTemplateTrackerMI::getMI256
double getMI256(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:788
vpTemplateTracker::templateSize
unsigned int templateSize
Definition: vpTemplateTracker.h:69
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
vpImageFilter::filter
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M)
Definition: vpImageFilter.cpp:97
vpTemplateTrackerMI::computeMI
void computeMI(double &MI)
Definition: vpTemplateTrackerMI.cpp:421
vpTemplateTrackerMI::getNormalizedCost
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:240
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:152
vpTemplateTrackerMI::computeHessienNormalized
void computeHessienNormalized(vpMatrix &H)
Definition: vpTemplateTrackerMI.cpp:510
vpTemplateTrackerMI::setBspline
void setBspline(const vpBsplineType &newbs)
Definition: vpTemplateTrackerMI.cpp:43
vpTemplateTracker::Warp
vpTemplateTrackerWarp * Warp
Definition: vpTemplateTracker.h:121
vpTemplateTrackerMI::computeProba
void computeProba(int &nbpoint)
Definition: vpTemplateTrackerMI.cpp:331
vpTemplateTrackerMI::hessianComputation
vpHessienType hessianComputation
Definition: vpTemplateTrackerMI.h:79
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217