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