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