Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpTemplateTrackerMI.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  *
38 *****************************************************************************/
39 #include <visp3/core/vpException.h>
40 #include <visp3/tt_mi/vpTemplateTrackerMI.h>
41 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
42 
44 {
45  bspline = (int)newbs;
47  Ncb = Nc + bspline;
48  if (Pt)
49  delete[] Pt;
50  if (Pr)
51  delete[] Pr;
52  if (Prt)
53  delete[] Prt;
54  if (dPrt)
55  delete[] dPrt;
56  if (d2Prt)
57  delete[] d2Prt;
58  if (PrtD)
59  delete[] PrtD;
60  if (dPrtD)
61  delete[] dPrtD;
62  if (PrtTout)
63  delete[] PrtTout;
64 
65  Pt = new double[Ncb];
66  Pr = new double[Ncb];
67 
68  Prt = new double[Ncb * Ncb];
69  dPrt = new double[Ncb * Ncb * (int)(nbParam)];
70  d2Prt = new double[Ncb * Ncb * (int)(nbParam * nbParam)];
71 
72  PrtD = new double[Nc * Nc * influBspline];
73  dPrtD = new double[Nc * Nc * (int)(nbParam)*influBspline];
74  PrtTout = new double[Nc * Nc * influBspline * (1 + (int)(nbParam + nbParam * nbParam))];
75 
77 }
78 
80  : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0), temp(nullptr),
81  Prt(nullptr), dPrt(nullptr), Pt(nullptr), Pr(nullptr), d2Prt(nullptr), PrtTout(nullptr), dprtemp(nullptr), PrtD(nullptr), dPrtD(nullptr),
82  influBspline(0), bspline(3), Nc(8), Ncb(0), d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
83  NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
84 {
85  Ncb = Nc + bspline;
87 
88  dW.resize(2, nbParam);
90  G.resize(nbParam);
94  dprtemp = new double[nbParam];
95  temp = new double[nbParam];
96 
97  X1.resize(2);
98  X2.resize(2);
99 
100  PrtD = new double[Nc * Nc * influBspline]; //(r,t)
101  dPrtD = new double[Nc * Nc * (int)(nbParam)*influBspline];
102 
103  Prt = new double[Ncb * Ncb]; //(r,t)
104  Pt = new double[Ncb];
105  Pr = new double[Ncb];
106  dPrt = new double[Ncb * Ncb * (int)(nbParam)];
107  d2Prt = new double[Ncb * Ncb * (int)(nbParam * nbParam)];
108 
109  PrtTout = new double[Nc * Nc * influBspline * (1 + (int)(nbParam + nbParam * nbParam))];
110 
111  lambda = lambdaDep;
112 }
113 
115 {
116  Nc = nc;
117  Ncb = Nc + bspline;
118 
119  if (Pt)
120  delete[] Pt;
121  if (Pr)
122  delete[] Pr;
123  if (Prt)
124  delete[] Prt;
125  if (dPrt)
126  delete[] dPrt;
127  if (d2Prt)
128  delete[] d2Prt;
129  if (PrtD)
130  delete[] PrtD;
131  if (dPrtD)
132  delete[] dPrtD;
133  if (PrtTout)
134  delete[] PrtTout;
135 
136  PrtD = new double[Nc * Nc * influBspline]; //(r,t)
137  dPrtD = new double[Nc * Nc * (int)(nbParam)*influBspline];
138  Prt = new double[Ncb * Ncb]; //(r,t)
139  dPrt = new double[Ncb * Ncb * (int)(nbParam)];
140  Pt = new double[Ncb];
141  Pr = new double[Ncb];
142  d2Prt = new double[Ncb * Ncb * (int)(nbParam * nbParam)]; //(r,t)
143  PrtTout = new double[Nc * Nc * influBspline * (1 + (int)(nbParam + nbParam * nbParam))];
144 }
145 
147 {
148  double MI = 0;
149  int Nbpoint = 0;
150  double IW;
151 
152  unsigned int Ncb_ = (unsigned int)Ncb;
153  unsigned int Nc_ = (unsigned int)Nc;
154  unsigned int influBspline_ = (unsigned int)influBspline;
155 
156  memset(Prt, 0, Ncb_ * Ncb_ * sizeof(double));
157  memset(PrtD, 0, Nc_ * Nc_ * influBspline_ * sizeof(double));
158 
159  Warp->computeCoeff(tp);
160  for (unsigned int point = 0; point < templateSize; point++) {
161  X1[0] = ptTemplate[point].x;
162  X1[1] = ptTemplate[point].y;
163 
164  Warp->computeDenom(X1, tp);
165  Warp->warpX(X1, X2, tp);
166  double j2 = X2[0];
167  double i2 = X2[1];
168 
169  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
170  Nbpoint++;
171 
172  double Tij = ptTemplate[point].val;
173  if (!blur)
174  IW = I.getValue(i2, j2);
175  else
176  IW = BI.getValue(i2, j2);
177 
178  double Nc_1 = (Nc - 1.) / 255.;
179  double IW_Nc = IW * Nc_1;
180  double Tij_Nc = Tij * Nc_1;
181  int cr = static_cast<int>(IW_Nc);
182  int ct = static_cast<int>(Tij_Nc);
183  double er = IW_Nc - cr;
184  double et = Tij_Nc - ct;
185 
186  // Calcul de l'histogramme joint par interpolation bilineaire
187  // (Bspline ordre 1)
188  vpTemplateTrackerMIBSpline::PutPVBsplineD(PrtD, cr, er, ct, et, Nc, 1., bspline);
189  }
190  }
191 
192  ratioPixelIn = (double)Nbpoint / (double)templateSize;
193 
194  double *pt = PrtD;
195  for (int r = 0; r < Nc; r++)
196  for (int t = 0; t < Nc; t++) {
197  for (int i = 0; i < influBspline; i++) {
198  int r2, t2;
199  r2 = r + i / bspline;
200  t2 = t + i % bspline;
201  Prt[r2 * Ncb + t2] += *pt;
202 
203  pt++;
204  }
205  }
206 
207  if (Nbpoint == 0)
208  return 0;
209  for (unsigned int r = 0; r < Ncb_; r++) {
210  for (unsigned int t = 0; t < Ncb_; t++) {
211  Prt[r * Ncb_ + t] /= Nbpoint;
212  }
213  }
214  // Compute Pr, Pt
215  memset(Pr, 0, Ncb_ * sizeof(double));
216  memset(Pt, 0, Ncb_ * sizeof(double));
217  for (unsigned int r = 0; r < Ncb_; r++) {
218  for (unsigned int t = 0; t < Ncb_; t++) {
219  Pr[r] += Prt[r * Ncb_ + t];
220  Pt[r] += Prt[t * Ncb_ + r];
221  }
222  }
223 
224  for (unsigned int r = 0; r < Ncb_; r++) {
225  if (std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon()) {
226  MI -= Pr[r] * log(Pr[r]);
227  }
228  if (std::fabs(Pt[r]) > std::numeric_limits<double>::epsilon()) {
229  MI -= Pt[r] * log(Pt[r]);
230  }
231  for (unsigned int t = 0; t < Ncb_; t++) {
232  unsigned int r_Ncb_t_ = r * Ncb_ + t;
233  if (std::fabs(Prt[r_Ncb_t_]) > std::numeric_limits<double>::epsilon()) {
234  MI += Prt[r_Ncb_t_] * log(Prt[r_Ncb_t_]);
235  }
236  }
237  }
238 
239  return -MI;
240 }
241 
243 {
244  // Attention, cette version calculee de la NMI ne pourra pas atteindre le
245  // maximum de 2. Ceci est du au fait que par defaut, l'image est floutee dans
246  // vpTemplateTracker::initTracking()
247 
248  double MI = 0;
249  double Nbpoint = 0;
250  double IW;
251 
252  double Pr_[256];
253  double Pt_[256];
254  double Prt_[256][256];
255 
256  memset(Pr_, 0, 256 * sizeof(double));
257  memset(Pt_, 0, 256 * sizeof(double));
258  memset(Prt_, 0, 256 * 256 * sizeof(double));
259 
260  for (unsigned int point = 0; point < templateSize; point++) {
261  X1[0] = ptTemplate[point].x;
262  X1[1] = ptTemplate[point].y;
263 
264  Warp->computeDenom(X1, tp);
265  Warp->warpX(X1, X2, tp);
266  double j2 = X2[0];
267  double i2 = X2[1];
268 
269  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
270  Nbpoint++;
271  double Tij = ptTemplate[point].val;
272  int Tij_ = static_cast<int>(Tij);
273  if (!blur) {
274  IW = I[(int)i2][(int)j2];
275  }
276  else {
277  IW = BI.getValue(i2, j2);
278  }
279  int IW_ = static_cast<int>(IW);
280 
281  Pr_[Tij_]++;
282  Pt_[IW_]++;
283  Prt_[Tij_][IW_]++;
284  }
285  }
286 
287  double denom = 0;
288  for (int r = 0; r < 256; r++) {
289  Pr_[r] /= Nbpoint;
290  Pt_[r] /= Nbpoint;
291  if (std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon()) {
292  MI -= Pr_[r] * log(Pr_[r]);
293  }
294  if (std::fabs(Pt_[r]) > std::numeric_limits<double>::epsilon()) {
295  MI -= Pt_[r] * log(Pt_[r]);
296  }
297  for (int t = 0; t < 256; t++) {
298  Prt_[r][t] /= Nbpoint;
299  if (std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon()) {
300  denom -= (Prt_[r][t] * log(Prt_[r][t]));
301  }
302  }
303  }
304 
305  if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
306  MI = MI / denom;
307  else
308  MI = 0;
309 
310  return -MI;
311 }
312 
314 {
315  if (Pt)
316  delete[] Pt;
317  if (Pr)
318  delete[] Pr;
319  if (Prt)
320  delete[] Prt;
321  if (dPrt)
322  delete[] dPrt;
323  if (d2Prt)
324  delete[] d2Prt;
325  if (PrtD)
326  delete[] PrtD;
327  if (dPrtD)
328  delete[] dPrtD;
329  if (PrtTout)
330  delete[] PrtTout;
331  if (temp)
332  delete[] temp;
333  if (dprtemp)
334  delete[] dprtemp;
335 }
336 
338 {
339  double *pt = PrtTout;
340  unsigned int Nc_ = static_cast<unsigned int>(Nc);
341  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
342  unsigned int bspline_ = static_cast<unsigned int>(bspline);
343 
344  for (unsigned int r = 0; r < Nc_; r++) {
345  for (unsigned int t = 0; t < Nc_; t++) {
346  for (unsigned int r2 = 0; r2 < bspline_; r2++) {
347  unsigned int r2_r_Ncb_ = (r2 + r) * Ncb_;
348  for (unsigned int t2 = 0; t2 < bspline_; t2++) {
349  unsigned int t2_t_ = t2 + t;
350  unsigned int r2_r_Ncb_t2_t_nbParam_ = (r2_r_Ncb_ + t2_t_) * nbParam;
351  Prt[r2_r_Ncb_ + t2_t_] += *pt++;
352  for (unsigned int ip = 0; ip < nbParam; ip++) {
353  dPrt[r2_r_Ncb_t2_t_nbParam_ + ip] += *pt++;
354  unsigned int ip_nbParam_ = ip * nbParam;
355  for (unsigned int it = 0; it < nbParam; it++) {
356  d2Prt[r2_r_Ncb_t2_t_nbParam_ * nbParam + ip_nbParam_ + it] += *pt++;
357  }
358  }
359  }
360  }
361  }
362  }
363 
364  if (nbpoint == 0) {
365  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
366  }
367  unsigned int indd, indd2;
368  indd = indd2 = 0;
369  for (volatile int i = 0; i < Ncb * Ncb; i++) {
370  Prt[i] = Prt[i] / nbpoint;
371  for (unsigned int j = 0; j < nbParam; j++) {
372  dPrt[indd] /= nbpoint;
373  indd++;
374  for (unsigned int k = 0; k < nbParam; k++) {
375  d2Prt[indd2] /= nbpoint;
376  indd2++;
377  }
378  }
379  }
380 }
381 
383 {
384  unsigned int Ncb_ = (unsigned int)Ncb;
385 
386  // Compute Pr and Pt
387  memset(Pr, 0, Ncb_ * sizeof(double));
388  memset(Pt, 0, Ncb_ * sizeof(double));
389  for (unsigned int r = 0; r < Ncb_; r++) {
390  unsigned int r_Nbc_ = r * Ncb_;
391  for (unsigned int t = 0; t < Ncb_; t++) {
392  Pr[r] += Prt[r_Nbc_ + t];
393  Pt[r] += Prt[r + Ncb_ * t];
394  }
395  }
396 
397  // Compute Entropy
398  for (unsigned int r = 0; r < Ncb_; r++) {
399  if (std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon()) {
400  MI -= Pr[r] * log(Pr[r]);
401  }
402  if (std::fabs(Pt[r]) > std::numeric_limits<double>::epsilon()) {
403  MI -= Pt[r] * log(Pt[r]);
404  }
405  unsigned int r_Nbc_ = r * Ncb_;
406  for (unsigned int t = 0; t < Ncb_; t++) {
407  unsigned int r_Nbc_t_ = r_Nbc_ + t;
408  if (std::fabs(Prt[r_Nbc_t_]) > std::numeric_limits<double>::epsilon()) {
409  MI += Prt[r_Nbc_t_] * log(Prt[r_Nbc_t_]);
410  }
411  }
412  }
413 }
414 
416 {
417  double seuilevitinf = 1e-200;
418  Hessian = 0;
419  double dtemp;
420  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
421  unsigned int nbParam2 = nbParam * nbParam;
422 
423  for (unsigned int t = 0; t < Ncb_; t++) {
424  if (Pt[t] > seuilevitinf) {
425  for (unsigned int r = 0; r < Ncb_; r++) {
426  if (Prt[r * Ncb_ + t] > seuilevitinf) {
427  unsigned int r_Ncb_t_ = r * Ncb_ + t;
428  unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ * nbParam;
429  for (unsigned int it = 0; it < nbParam; it++) {
430  dprtemp[it] = dPrt[r_Ncb_t_nbParam_ + it];
431  }
432 
433  dtemp = 1. + log(Prt[r * Ncb_ + t] / Pt[t]);
434 
435  double Prt_Pt_ = 1. / Prt[r_Ncb_t_] - 1. / Pt[t];
436  unsigned int r_Ncb_t_nbParam2_ = r_Ncb_t_ * nbParam2;
437  for (unsigned int it = 0; it < nbParam; it++) {
438  unsigned int r_Ncb_t_nbParam2_it_nbParam_ = r_Ncb_t_nbParam2_ + it * nbParam;
439  for (unsigned int jt = 0; jt < nbParam; jt++) {
441  Hessian[it][jt] +=
442  dprtemp[it] * dprtemp[jt] * Prt_Pt_ + d2Prt[r_Ncb_t_nbParam2_it_nbParam_ + jt] * dtemp;
443  else if (ApproxHessian == HESSIAN_NEW)
444  Hessian[it][jt] += d2Prt[r_Ncb_t_nbParam2_it_nbParam_ + jt] * dtemp;
445  else
446  Hessian[it][jt] += dprtemp[it] * dprtemp[jt] * Prt_Pt_;
447  }
448  }
449  }
450  }
451  }
452  }
453 }
454 
456 {
457  double seuilevitinf = 1e-200;
458  double u = 0, v = 0, B = 0;
459  m_du.resize(nbParam);
460  m_dv.resize(nbParam);
461  m_A.resize(nbParam);
462  m_dB.resize(nbParam);
463  m_d2u.resize(nbParam);
464  m_d2v.resize(nbParam);
465  m_dA.resize(nbParam);
466 
467  for (unsigned int i = 0; i < nbParam; i++) {
468  m_d2u[i].resize(nbParam);
469  m_d2v[i].resize(nbParam);
470  m_dA[i].resize(nbParam);
471  }
472 
473  std::fill(m_du.begin(), m_du.end(), 0);
474  std::fill(m_dv.begin(), m_dv.end(), 0);
475  std::fill(m_A.begin(), m_A.end(), 0);
476  std::fill(m_dB.begin(), m_dB.end(), 0);
477  for (unsigned int it = 0; it < nbParam; it++) {
478  std::fill(m_d2u[0].begin(), m_d2u[0].end(), 0);
479  std::fill(m_d2v[0].begin(), m_d2v[0].end(), 0);
480  std::fill(m_dA[0].begin(), m_dA[0].end(), 0);
481  }
482 
483  memset(dprtemp, 0, nbParam * sizeof(double));
484 
485  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
486  unsigned int nbParam2 = nbParam * nbParam;
487 
488  for (unsigned int t = 0; t < Ncb_; t++) {
489  if (Pt[t] > seuilevitinf) {
490  for (unsigned int r = 0; r < Ncb_; r++) {
491  unsigned int r_Ncb_t_ = r * Ncb_ + t;
492  if (Prt[r_Ncb_t_] > seuilevitinf) {
493  unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ * nbParam;
494  for (unsigned int it = 0; it < nbParam; it++) {
495  // dPxy/dt
496  dprtemp[it] = dPrt[r_Ncb_t_nbParam_ + it];
497  }
498  double log_Pt_Pr_ = log(Pt[t] * Pr[r]);
499  double log_Prt_ = log(Prt[r_Ncb_t_]);
500  // u = som(Pxy.logPxPy)
501  u += Prt[r_Ncb_t_] * log_Pt_Pr_;
502  // v = som(Pxy.logPxy)
503  v += Prt[r_Ncb_t_] * log_Prt_;
504 
505  double log_Prt_1_ = 1 + log(Prt[r_Ncb_t_]);
506  for (unsigned int it = 0; it < nbParam; it++) {
507  // u' = som dPxylog(PxPy)
508  m_du[it] += dprtemp[it] * log_Pt_Pr_;
509  // v' = som dPxy(1+log(Pxy))
510  m_dv[it] += dprtemp[it] * log_Prt_1_;
511  }
512  double Prt_ = 1.0 / Prt[r_Ncb_t_];
513  unsigned int r_Ncb_t_nbParam2_ = r_Ncb_t_ * nbParam2;
514  for (unsigned int it = 0; it < nbParam; it++) {
515  double dprtemp_it2_ = Prt_ * dprtemp[it] * dprtemp[it];
516  unsigned int r_Ncb_t_nbParam2_it_nbParam_ = r_Ncb_t_nbParam2_ + it * nbParam;
517  for (unsigned int jt = 0; jt < nbParam; jt++) {
518  unsigned int r_Ncb_t_nbParam2_it_nbParam_jt_ = r_Ncb_t_nbParam2_it_nbParam_ + jt;
519  m_d2u[it][jt] += d2Prt[r_Ncb_t_nbParam2_it_nbParam_jt_] * log_Pt_Pr_ + dprtemp_it2_;
520  m_d2v[it][jt] += d2Prt[r_Ncb_t_nbParam2_it_nbParam_jt_] * log_Prt_1_ + dprtemp_it2_;
521  }
522  }
523  }
524  }
525  }
526  }
527  // B = v2
528  B = (v * v);
529  double B2 = B * B;
530  for (unsigned int it = 0; it < nbParam; it++) {
531  // A = u'v-uv'
532  m_A[it] = m_du[it] * v - u * m_dv[it];
533  // B' = 2vv'
534  m_dB[it] = 2 * v * m_dv[it];
535  double A_it_dB_it_ = m_A[it] * m_dB[it];
536  for (unsigned int jt = 0; jt < nbParam; jt++) {
537  // A' = u''v-v''u
538  m_dA[it][jt] = m_d2u[it][jt] * v - m_d2v[it][jt] * u;
539  // Hessian = (A'B-AB')/B2
540  Hessian[it][jt] = (m_dA[it][jt] * B - A_it_dB_it_) / B2;
541  }
542  }
543 }
544 
546 {
547  double seuilevitinf = 1e-200;
548  G = 0;
549  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
550  double dtemp;
551  for (unsigned int t = 0; t < Ncb_; t++) {
552  if (Pt[t] > seuilevitinf) {
553  for (unsigned int r = 0; r < Ncb_; r++) {
554  unsigned int r_Ncb_t_ = r * Ncb_ + t;
555  if (Prt[r_Ncb_t_] > seuilevitinf) {
556  unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ * nbParam;
557  for (unsigned int it = 0; it < nbParam; it++) {
558  dprtemp[it] = dPrt[r_Ncb_t_nbParam_ + it];
559  }
560 
561  dtemp = 1. + log(Prt[r_Ncb_t_] / Pt[t]);
562 
563  for (unsigned int it = 0; it < nbParam; it++) {
564  G[it] += dtemp * dprtemp[it];
565  }
566  }
567  }
568  }
569  }
570 }
571 
573 {
574  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
575  unsigned int Nc_ = static_cast<unsigned int>(Nc);
576  unsigned int influBspline_ = static_cast<unsigned int>(influBspline);
577 
578  unsigned int Ncb2_ = Ncb_ * Ncb_ * sizeof(double);
579  unsigned int Ncb2_nbParam_ = Ncb2_ * nbParam;
580  unsigned int Ncb2_nbParam2_ = Ncb2_nbParam_ * nbParam;
581 
582  memset(Prt, 0, Ncb2_);
583  memset(dPrt, 0, Ncb2_nbParam_);
584  memset(d2Prt, 0, Ncb2_nbParam2_);
585  memset(PrtTout, 0, Nc_ * Nc_ * influBspline_ * (1 + nbParam + nbParam * nbParam) * sizeof(double));
586 }
587 
588 double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I, int &nc, const int &bspline_, vpColVector &tp)
589 {
590  unsigned int tNcb = static_cast<unsigned int>(nc + bspline_);
591  unsigned int tinfluBspline = static_cast<unsigned int>(bspline_ * bspline_);
592  unsigned int nc_ = static_cast<unsigned int>(nc);
593 
594  double *tPrtD = new double[nc_ * nc_ * tinfluBspline];
595  double *tPrt = new double[tNcb * tNcb];
596  double *tPr = new double[tNcb];
597  double *tPt = new double[tNcb];
598 
599  double MI = 0;
600  volatile int Nbpoint = 0;
601  double IW;
602 
603  vpImage<double> GaussI;
604  vpImageFilter::filter(I, GaussI, fgG, taillef);
605 
606  memset(tPrt, 0, tNcb * tNcb * sizeof(double));
607  memset(tPrtD, 0, nc_ * nc_ * tinfluBspline * sizeof(double));
608 
609  Warp->computeCoeff(tp);
610  for (unsigned int point = 0; point < templateSize; point++) {
611  X1[0] = ptTemplate[point].x;
612  X1[1] = ptTemplate[point].y;
613 
614  Warp->computeDenom(X1, tp);
615  Warp->warpX(X1, X2, tp);
616  double j2 = X2[0];
617  double i2 = X2[1];
618 
619  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth()) - 1) {
620  Nbpoint++;
621 
622  double Tij = ptTemplate[point].val;
623  if (!blur)
624  IW = I.getValue(i2, j2);
625  else
626  IW = GaussI.getValue(i2, j2);
627 
628  int cr = static_cast<int>((IW * (nc - 1)) / 255.);
629  int ct = static_cast<int>((Tij * (nc - 1)) / 255.);
630  double er = (IW * (nc - 1)) / 255. - cr;
631  double et = (Tij * (nc - 1)) / 255. - ct;
632 
633  // Calcul de l'histogramme joint par interpolation bilineaire (Bspline_
634  // ordre 1)
635  vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1., bspline_);
636  }
637  }
638  double *pt = tPrtD;
639  int tNcb_ = (int)tNcb;
640  int tinfluBspline_ = (int)tinfluBspline;
641  for (int r = 0; r < nc; r++)
642  for (int t = 0; t < nc; t++) {
643  for (volatile int i = 0; i < tinfluBspline_; i++) {
644  int r2, t2;
645  r2 = r + i / bspline_;
646  t2 = t + i % bspline_;
647  tPrt[r2 * tNcb_ + t2] += *pt;
648 
649  pt++;
650  }
651  }
652 
653  if (Nbpoint == 0) {
654  delete[] tPrtD;
655  delete[] tPrt;
656  delete[] tPr;
657  delete[] tPt;
658 
659  return 0;
660  }
661  else {
662  for (unsigned int r = 0; r < tNcb; r++)
663  for (unsigned int t = 0; t < tNcb; t++)
664  tPrt[r * tNcb + t] = tPrt[r * tNcb + t] / Nbpoint;
665  // calcul Pr;
666  memset(tPr, 0, tNcb * sizeof(double));
667  for (unsigned int r = 0; r < tNcb; r++) {
668  for (unsigned int t = 0; t < tNcb; t++)
669  tPr[r] += tPrt[r * tNcb + t];
670  }
671 
672  // calcul Pt;
673  memset(tPt, 0, (size_t)(tNcb * sizeof(double)));
674  for (unsigned int t = 0; t < tNcb; t++) {
675  for (unsigned int r = 0; r < tNcb; r++)
676  tPt[t] += tPrt[r * tNcb + t];
677  }
678  for (unsigned int r = 0; r < tNcb; r++)
679  if (std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
680  MI -= tPr[r] * log(tPr[r]);
681 
682  for (unsigned int t = 0; t < tNcb; t++)
683  if (std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
684  MI -= tPt[t] * log(tPt[t]);
685 
686  for (unsigned int r = 0; r < tNcb; r++)
687  for (unsigned int t = 0; t < tNcb; t++)
688  if (std::fabs(tPrt[r * tNcb + t]) > std::numeric_limits<double>::epsilon())
689  MI += tPrt[r * tNcb + t] * log(tPrt[r * tNcb + t]);
690  }
691  delete[] tPrtD;
692  delete[] tPrt;
693  delete[] tPr;
694  delete[] tPt;
695 
696  return MI;
697 }
698 
700 {
701  vpMatrix Prt256(256, 256);
702  Prt256 = 0;
703  vpColVector Pr256(256);
704  Pr256 = 0;
705  vpColVector Pt256(256);
706  Pt256 = 0;
707 
708  volatile int Nbpoint = 0;
709  unsigned int Tij, IW;
710 
711  vpImage<double> GaussI;
712  if (blur)
713  vpImageFilter::filter(I, GaussI, fgG, taillef);
714 
715  Warp->computeCoeff(tp);
716  for (unsigned int point = 0; point < templateSize; point++) {
717  X1[0] = ptTemplate[point].x;
718  X1[1] = ptTemplate[point].y;
719 
720  Warp->computeDenom(X1, tp);
721  Warp->warpX(X1, X2, tp);
722  double j2 = X2[0];
723  double i2 = X2[1];
724 
725  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth()) - 1) {
726  Nbpoint++;
727 
728  Tij = static_cast<unsigned int>(ptTemplate[point].val);
729  if (!blur)
730  IW = static_cast<unsigned int>(I.getValue(i2, j2));
731  else
732  IW = static_cast<unsigned int>(GaussI.getValue(i2, j2));
733 
734  Prt256[Tij][IW]++;
735  Pr256[Tij]++;
736  Pt256[IW]++;
737  }
738  }
739 
740  if (Nbpoint == 0) {
741  throw(vpException(vpException::divideByZeroError, "Cannot get MI; number of points = 0"));
742  }
743  Prt256 = Prt256 / Nbpoint;
744  Pr256 = Pr256 / Nbpoint;
745  Pt256 = Pt256 / Nbpoint;
746 
747  double MI = 0;
748 
749  for (unsigned int t = 0; t < 256; t++) {
750  for (unsigned int r = 0; r < 256; r++) {
751  if (std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
752  MI += Prt256[r][t] * log(Prt256[r][t]);
753  }
754  if (std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
755  MI += -Pt256[t] * log(Pt256[t]);
756  if (std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
757  MI += -Pr256[t] * log(Pr256[t]);
758  }
759  return MI;
760 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
static void filter(const vpImage< ImageType > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false, const vpImage< bool > *p_mask=nullptr)
unsigned int getWidth() const
Definition: vpImage.h:249
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1816
unsigned int getHeight() const
Definition: vpImage.h:184
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpHessienApproximationType ApproxHessian
void computeHessienNormalized(vpMatrix &H)
std::vector< std::vector< double > > m_d2v
vpHessienType hessianComputation
void computeMI(double &MI)
std::vector< double > m_dB
std::vector< double > m_du
void computeProba(int &nbpoint)
vpTemplateTrackerMI()
Default constructor.
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp) vp_override
void computeHessien(vpMatrix &H)
std::vector< double > m_dv
std::vector< std::vector< double > > m_d2u
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
virtual ~vpTemplateTrackerMI() vp_override
std::vector< std::vector< double > > m_dA
void setBspline(const vpBsplineType &newbs)
double getMI256(const vpImage< unsigned char > &I, const vpColVector &tp)
std::vector< double > m_A
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
vpImage< double > BI
unsigned int templateSize
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.