Visual Servoing Platform  version 3.6.0 under development (2023-09-27)
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 void vpTemplateTrackerMI::setBspline(const vpBsplineType &newbs)
44 {
45  bspline = (int)newbs;
46  influBspline = bspline * bspline;
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 
76  hessianComputation = USE_HESSIEN_DESIRE;
77 }
78 
79 vpTemplateTrackerMI::vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp)
80  : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0), temp(NULL),
81  Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL), dprtemp(NULL), PrtD(NULL), dPrtD(NULL),
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;
86  influBspline = bspline * bspline;
87 
88  dW.resize(2, nbParam);
89  H.resize(nbParam, nbParam);
90  G.resize(nbParam);
91  Hdesire.resize(nbParam, nbParam);
92  HLM.resize(nbParam, nbParam);
93  HLMdesire.resize(nbParam, 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 
114 void vpTemplateTrackerMI::setNc(int nc)
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 
146 double vpTemplateTrackerMI::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
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 
242 double vpTemplateTrackerMI::getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp)
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  } else {
276  IW = BI.getValue(i2, j2);
277  }
278  int IW_ = static_cast<int>(IW);
279 
280  Pr_[Tij_]++;
281  Pt_[IW_]++;
282  Prt_[Tij_][IW_]++;
283  }
284  }
285 
286  double denom = 0;
287  for (int r = 0; r < 256; r++) {
288  Pr_[r] /= Nbpoint;
289  Pt_[r] /= Nbpoint;
290  if (std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon()) {
291  MI -= Pr_[r] * log(Pr_[r]);
292  }
293  if (std::fabs(Pt_[r]) > std::numeric_limits<double>::epsilon()) {
294  MI -= Pt_[r] * log(Pt_[r]);
295  }
296  for (int t = 0; t < 256; t++) {
297  Prt_[r][t] /= Nbpoint;
298  if (std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon()) {
299  denom -= (Prt_[r][t] * log(Prt_[r][t]));
300  }
301  }
302  }
303 
304  if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
305  MI = MI / denom;
306  else
307  MI = 0;
308 
309  return -MI;
310 }
311 
312 vpTemplateTrackerMI::~vpTemplateTrackerMI()
313 {
314  if (Pt)
315  delete[] Pt;
316  if (Pr)
317  delete[] Pr;
318  if (Prt)
319  delete[] Prt;
320  if (dPrt)
321  delete[] dPrt;
322  if (d2Prt)
323  delete[] d2Prt;
324  if (PrtD)
325  delete[] PrtD;
326  if (dPrtD)
327  delete[] dPrtD;
328  if (PrtTout)
329  delete[] PrtTout;
330  if (temp)
331  delete[] temp;
332  if (dprtemp)
333  delete[] dprtemp;
334 }
335 
336 void vpTemplateTrackerMI::computeProba(int &nbpoint)
337 {
338  double *pt = PrtTout;
339  unsigned int Nc_ = static_cast<unsigned int>(Nc);
340  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
341  unsigned int bspline_ = static_cast<unsigned int>(bspline);
342 
343  for (unsigned int r = 0; r < Nc_; r++) {
344  for (unsigned int t = 0; t < Nc_; t++) {
345  for (unsigned int r2 = 0; r2 < bspline_; r2++) {
346  unsigned int r2_r_Ncb_ = (r2 + r) * Ncb_;
347  for (unsigned int t2 = 0; t2 < bspline_; t2++) {
348  unsigned int t2_t_ = t2 + t;
349  unsigned int r2_r_Ncb_t2_t_nbParam_ = (r2_r_Ncb_ + t2_t_) * nbParam;
350  Prt[r2_r_Ncb_ + t2_t_] += *pt++;
351  for (unsigned int ip = 0; ip < nbParam; ip++) {
352  dPrt[r2_r_Ncb_t2_t_nbParam_ + ip] += *pt++;
353  unsigned int ip_nbParam_ = ip * nbParam;
354  for (unsigned int it = 0; it < nbParam; it++) {
355  d2Prt[r2_r_Ncb_t2_t_nbParam_ * nbParam + ip_nbParam_ + it] += *pt++;
356  }
357  }
358  }
359  }
360  }
361  }
362 
363  if (nbpoint == 0) {
364  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
365  }
366  unsigned int indd, indd2;
367  indd = indd2 = 0;
368  for (volatile int i = 0; i < Ncb * Ncb; i++) {
369  Prt[i] = Prt[i] / nbpoint;
370  for (unsigned int j = 0; j < nbParam; j++) {
371  dPrt[indd] /= nbpoint;
372  indd++;
373  for (unsigned int k = 0; k < nbParam; k++) {
374  d2Prt[indd2] /= nbpoint;
375  indd2++;
376  }
377  }
378  }
379 }
380 
381 void vpTemplateTrackerMI::computeMI(double &MI)
382 {
383  unsigned int Ncb_ = (unsigned int)Ncb;
384 
385  // Compute Pr and Pt
386  memset(Pr, 0, Ncb_ * sizeof(double));
387  memset(Pt, 0, Ncb_ * sizeof(double));
388  for (unsigned int r = 0; r < Ncb_; r++) {
389  unsigned int r_Nbc_ = r * Ncb_;
390  for (unsigned int t = 0; t < Ncb_; t++) {
391  Pr[r] += Prt[r_Nbc_ + t];
392  Pt[r] += Prt[r + Ncb_ * t];
393  }
394  }
395 
396  // Compute Entropy
397  for (unsigned int r = 0; r < Ncb_; r++) {
398  if (std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon()) {
399  MI -= Pr[r] * log(Pr[r]);
400  }
401  if (std::fabs(Pt[r]) > std::numeric_limits<double>::epsilon()) {
402  MI -= Pt[r] * log(Pt[r]);
403  }
404  unsigned int r_Nbc_ = r * Ncb_;
405  for (unsigned int t = 0; t < Ncb_; t++) {
406  unsigned int r_Nbc_t_ = r_Nbc_ + t;
407  if (std::fabs(Prt[r_Nbc_t_]) > std::numeric_limits<double>::epsilon()) {
408  MI += Prt[r_Nbc_t_] * log(Prt[r_Nbc_t_]);
409  }
410  }
411  }
412 }
413 
414 void vpTemplateTrackerMI::computeHessien(vpMatrix &Hessian)
415 {
416  double seuilevitinf = 1e-200;
417  Hessian = 0;
418  double dtemp;
419  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
420  unsigned int nbParam2 = nbParam * nbParam;
421 
422  for (unsigned int t = 0; t < Ncb_; t++) {
423  if (Pt[t] > seuilevitinf) {
424  for (unsigned int r = 0; r < Ncb_; r++) {
425  if (Prt[r * Ncb_ + t] > seuilevitinf) {
426  unsigned int r_Ncb_t_ = r * Ncb_ + t;
427  unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ * nbParam;
428  for (unsigned int it = 0; it < nbParam; it++) {
429  dprtemp[it] = dPrt[r_Ncb_t_nbParam_ + it];
430  }
431 
432  dtemp = 1. + log(Prt[r * Ncb_ + t] / Pt[t]);
433 
434  double Prt_Pt_ = 1. / Prt[r_Ncb_t_] - 1. / Pt[t];
435  unsigned int r_Ncb_t_nbParam2_ = r_Ncb_t_ * nbParam2;
436  for (unsigned int it = 0; it < nbParam; it++) {
437  unsigned int r_Ncb_t_nbParam2_it_nbParam_ = r_Ncb_t_nbParam2_ + it * nbParam;
438  for (unsigned int jt = 0; jt < nbParam; jt++) {
439  if (ApproxHessian != HESSIAN_NONSECOND && ApproxHessian != HESSIAN_NEW)
440  Hessian[it][jt] +=
441  dprtemp[it] * dprtemp[jt] * Prt_Pt_ + d2Prt[r_Ncb_t_nbParam2_it_nbParam_ + jt] * dtemp;
442  else if (ApproxHessian == HESSIAN_NEW)
443  Hessian[it][jt] += d2Prt[r_Ncb_t_nbParam2_it_nbParam_ + jt] * dtemp;
444  else
445  Hessian[it][jt] += dprtemp[it] * dprtemp[jt] * Prt_Pt_;
446  }
447  }
448  }
449  }
450  }
451  }
452 }
453 
454 void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
455 {
456  double seuilevitinf = 1e-200;
457  double u = 0, v = 0, B = 0;
458  m_du.resize(nbParam);
459  m_dv.resize(nbParam);
460  m_A.resize(nbParam);
461  m_dB.resize(nbParam);
462  m_d2u.resize(nbParam);
463  m_d2v.resize(nbParam);
464  m_dA.resize(nbParam);
465 
466  for (unsigned int i = 0; i < nbParam; i++) {
467  m_d2u[i].resize(nbParam);
468  m_d2v[i].resize(nbParam);
469  m_dA[i].resize(nbParam);
470  }
471 
472  std::fill(m_du.begin(), m_du.end(), 0);
473  std::fill(m_dv.begin(), m_dv.end(), 0);
474  std::fill(m_A.begin(), m_A.end(), 0);
475  std::fill(m_dB.begin(), m_dB.end(), 0);
476  for (unsigned int it = 0; it < nbParam; it++) {
477  std::fill(m_d2u[0].begin(), m_d2u[0].end(), 0);
478  std::fill(m_d2v[0].begin(), m_d2v[0].end(), 0);
479  std::fill(m_dA[0].begin(), m_dA[0].end(), 0);
480  }
481 
482  memset(dprtemp, 0, nbParam * sizeof(double));
483 
484  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
485  unsigned int nbParam2 = nbParam * nbParam;
486 
487  for (unsigned int t = 0; t < Ncb_; t++) {
488  if (Pt[t] > seuilevitinf) {
489  for (unsigned int r = 0; r < Ncb_; r++) {
490  unsigned int r_Ncb_t_ = r * Ncb_ + t;
491  if (Prt[r_Ncb_t_] > seuilevitinf) {
492  unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ * nbParam;
493  for (unsigned int it = 0; it < nbParam; it++) {
494  // dPxy/dt
495  dprtemp[it] = dPrt[r_Ncb_t_nbParam_ + it];
496  }
497  double log_Pt_Pr_ = log(Pt[t] * Pr[r]);
498  double log_Prt_ = log(Prt[r_Ncb_t_]);
499  // u = som(Pxy.logPxPy)
500  u += Prt[r_Ncb_t_] * log_Pt_Pr_;
501  // v = som(Pxy.logPxy)
502  v += Prt[r_Ncb_t_] * log_Prt_;
503 
504  double log_Prt_1_ = 1 + log(Prt[r_Ncb_t_]);
505  for (unsigned int it = 0; it < nbParam; it++) {
506  // u' = som dPxylog(PxPy)
507  m_du[it] += dprtemp[it] * log_Pt_Pr_;
508  // v' = som dPxy(1+log(Pxy))
509  m_dv[it] += dprtemp[it] * log_Prt_1_;
510  }
511  double Prt_ = 1.0 / Prt[r_Ncb_t_];
512  unsigned int r_Ncb_t_nbParam2_ = r_Ncb_t_ * nbParam2;
513  for (unsigned int it = 0; it < nbParam; it++) {
514  double dprtemp_it2_ = Prt_ * dprtemp[it] * dprtemp[it];
515  unsigned int r_Ncb_t_nbParam2_it_nbParam_ = r_Ncb_t_nbParam2_ + it * nbParam;
516  for (unsigned int jt = 0; jt < nbParam; jt++) {
517  unsigned int r_Ncb_t_nbParam2_it_nbParam_jt_ = r_Ncb_t_nbParam2_it_nbParam_ + jt;
518  m_d2u[it][jt] += d2Prt[r_Ncb_t_nbParam2_it_nbParam_jt_] * log_Pt_Pr_ + dprtemp_it2_;
519  m_d2v[it][jt] += d2Prt[r_Ncb_t_nbParam2_it_nbParam_jt_] * log_Prt_1_ + dprtemp_it2_;
520  }
521  }
522  }
523  }
524  }
525  }
526  // B = v2
527  B = (v * v);
528  double B2 = B * B;
529  for (unsigned int it = 0; it < nbParam; it++) {
530  // A = u'v-uv'
531  m_A[it] = m_du[it] * v - u * m_dv[it];
532  // B' = 2vv'
533  m_dB[it] = 2 * v * m_dv[it];
534  double A_it_dB_it_ = m_A[it] * m_dB[it];
535  for (unsigned int jt = 0; jt < nbParam; jt++) {
536  // A' = u''v-v''u
537  m_dA[it][jt] = m_d2u[it][jt] * v - m_d2v[it][jt] * u;
538  // Hessian = (A'B-AB')/B2
539  Hessian[it][jt] = (m_dA[it][jt] * B - A_it_dB_it_) / B2;
540  }
541  }
542 }
543 
544 void vpTemplateTrackerMI::computeGradient()
545 {
546  double seuilevitinf = 1e-200;
547  G = 0;
548  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
549  double dtemp;
550  for (unsigned int t = 0; t < Ncb_; t++) {
551  if (Pt[t] > seuilevitinf) {
552  for (unsigned int r = 0; r < Ncb_; r++) {
553  unsigned int r_Ncb_t_ = r * Ncb_ + t;
554  if (Prt[r_Ncb_t_] > seuilevitinf) {
555  unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ * nbParam;
556  for (unsigned int it = 0; it < nbParam; it++) {
557  dprtemp[it] = dPrt[r_Ncb_t_nbParam_ + it];
558  }
559 
560  dtemp = 1. + log(Prt[r_Ncb_t_] / Pt[t]);
561 
562  for (unsigned int it = 0; it < nbParam; it++) {
563  G[it] += dtemp * dprtemp[it];
564  }
565  }
566  }
567  }
568  }
569 }
570 
571 void vpTemplateTrackerMI::zeroProbabilities()
572 {
573  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
574  unsigned int Nc_ = static_cast<unsigned int>(Nc);
575  unsigned int influBspline_ = static_cast<unsigned int>(influBspline);
576 
577  unsigned int Ncb2_ = Ncb_ * Ncb_ * sizeof(double);
578  unsigned int Ncb2_nbParam_ = Ncb2_ * nbParam;
579  unsigned int Ncb2_nbParam2_ = Ncb2_nbParam_ * nbParam;
580 
581  memset(Prt, 0, Ncb2_);
582  memset(dPrt, 0, Ncb2_nbParam_);
583  memset(d2Prt, 0, Ncb2_nbParam2_);
584  memset(PrtTout, 0, Nc_ * Nc_ * influBspline_ * (1 + nbParam + nbParam * nbParam) * sizeof(double));
585 }
586 
587 double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I, int &nc, const int &bspline_, vpColVector &tp)
588 {
589  unsigned int tNcb = static_cast<unsigned int>(nc + bspline_);
590  unsigned int tinfluBspline = static_cast<unsigned int>(bspline_ * bspline_);
591  unsigned int nc_ = static_cast<unsigned int>(nc);
592 
593  double *tPrtD = new double[nc_ * nc_ * tinfluBspline];
594  double *tPrt = new double[tNcb * tNcb];
595  double *tPr = new double[tNcb];
596  double *tPt = new double[tNcb];
597 
598  double MI = 0;
599  volatile int Nbpoint = 0;
600  double IW;
601 
602  vpImage<double> GaussI;
603  vpImageFilter::filter(I, GaussI, fgG, taillef);
604 
605  memset(tPrt, 0, tNcb * tNcb * sizeof(double));
606  memset(tPrtD, 0, nc_ * nc_ * tinfluBspline * sizeof(double));
607 
608  Warp->computeCoeff(tp);
609  for (unsigned int point = 0; point < templateSize; point++) {
610  X1[0] = ptTemplate[point].x;
611  X1[1] = ptTemplate[point].y;
612 
613  Warp->computeDenom(X1, tp);
614  Warp->warpX(X1, X2, tp);
615  double j2 = X2[0];
616  double i2 = X2[1];
617 
618  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth()) - 1) {
619  Nbpoint++;
620 
621  double Tij = ptTemplate[point].val;
622  if (!blur)
623  IW = I.getValue(i2, j2);
624  else
625  IW = GaussI.getValue(i2, j2);
626 
627  int cr = static_cast<int>((IW * (nc - 1)) / 255.);
628  int ct = static_cast<int>((Tij * (nc - 1)) / 255.);
629  double er = (IW * (nc - 1)) / 255. - cr;
630  double et = (Tij * (nc - 1)) / 255. - ct;
631 
632  // Calcul de l'histogramme joint par interpolation bilineaire (Bspline_
633  // ordre 1)
634  vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1., bspline_);
635  }
636  }
637  double *pt = tPrtD;
638  int tNcb_ = (int)tNcb;
639  int tinfluBspline_ = (int)tinfluBspline;
640  for (int r = 0; r < nc; r++)
641  for (int t = 0; t < nc; t++) {
642  for (volatile int i = 0; i < tinfluBspline_; i++) {
643  int r2, t2;
644  r2 = r + i / bspline_;
645  t2 = t + i % bspline_;
646  tPrt[r2 * tNcb_ + t2] += *pt;
647 
648  pt++;
649  }
650  }
651 
652  if (Nbpoint == 0) {
653  delete[] tPrtD;
654  delete[] tPrt;
655  delete[] tPr;
656  delete[] tPt;
657 
658  return 0;
659  } else {
660  for (unsigned int r = 0; r < tNcb; r++)
661  for (unsigned int t = 0; t < tNcb; t++)
662  tPrt[r * tNcb + t] = tPrt[r * tNcb + t] / Nbpoint;
663  // calcul Pr;
664  memset(tPr, 0, tNcb * sizeof(double));
665  for (unsigned int r = 0; r < tNcb; r++) {
666  for (unsigned int t = 0; t < tNcb; t++)
667  tPr[r] += tPrt[r * tNcb + t];
668  }
669 
670  // calcul Pt;
671  memset(tPt, 0, (size_t)(tNcb * sizeof(double)));
672  for (unsigned int t = 0; t < tNcb; t++) {
673  for (unsigned int r = 0; r < tNcb; r++)
674  tPt[t] += tPrt[r * tNcb + t];
675  }
676  for (unsigned int r = 0; r < tNcb; r++)
677  if (std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
678  MI -= tPr[r] * log(tPr[r]);
679 
680  for (unsigned int t = 0; t < tNcb; t++)
681  if (std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
682  MI -= tPt[t] * log(tPt[t]);
683 
684  for (unsigned int r = 0; r < tNcb; r++)
685  for (unsigned int t = 0; t < tNcb; t++)
686  if (std::fabs(tPrt[r * tNcb + t]) > std::numeric_limits<double>::epsilon())
687  MI += tPrt[r * tNcb + t] * log(tPrt[r * tNcb + t]);
688  }
689  delete[] tPrtD;
690  delete[] tPrt;
691  delete[] tPr;
692  delete[] tPt;
693 
694  return MI;
695 }
696 
697 double vpTemplateTrackerMI::getMI256(const vpImage<unsigned char> &I, const vpColVector &tp)
698 {
699  vpMatrix Prt256(256, 256);
700  Prt256 = 0;
701  vpColVector Pr256(256);
702  Pr256 = 0;
703  vpColVector Pt256(256);
704  Pt256 = 0;
705 
706  volatile int Nbpoint = 0;
707  unsigned int Tij, IW;
708 
709  vpImage<double> GaussI;
710  if (blur)
711  vpImageFilter::filter(I, GaussI, fgG, taillef);
712 
713  Warp->computeCoeff(tp);
714  for (unsigned int point = 0; point < templateSize; point++) {
715  X1[0] = ptTemplate[point].x;
716  X1[1] = ptTemplate[point].y;
717 
718  Warp->computeDenom(X1, tp);
719  Warp->warpX(X1, X2, tp);
720  double j2 = X2[0];
721  double i2 = X2[1];
722 
723  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth()) - 1) {
724  Nbpoint++;
725 
726  Tij = static_cast<unsigned int>(ptTemplate[point].val);
727  if (!blur)
728  IW = static_cast<unsigned int>(I.getValue(i2, j2));
729  else
730  IW = static_cast<unsigned int>(GaussI.getValue(i2, j2));
731 
732  Prt256[Tij][IW]++;
733  Pr256[Tij]++;
734  Pt256[IW]++;
735  }
736  }
737 
738  if (Nbpoint == 0) {
739  throw(vpException(vpException::divideByZeroError, "Cannot get MI; number of points = 0"));
740  }
741  Prt256 = Prt256 / Nbpoint;
742  Pr256 = Pr256 / Nbpoint;
743  Pt256 = Pt256 / Nbpoint;
744 
745  double MI = 0;
746 
747  for (unsigned int t = 0; t < 256; t++) {
748  for (unsigned int r = 0; r < 256; r++) {
749  if (std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
750  MI += Prt256[r][t] * log(Prt256[r][t]);
751  }
752  if (std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
753  MI += -Pt256[t] * log(Pt256[t]);
754  if (std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
755  MI += -Pr256[t] * log(Pr256[t]);
756  }
757  return MI;
758 }
vpArray2D::resize
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
vpColVector::resize
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:351
vpException
error that can be emitted by ViSP classes.
Definition: vpException.h:59
vpException::divideByZeroError
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
vpImageFilter::filter
static void filter(const vpImage< unsigned char > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
Definition: vpImageFilter.h:185
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:242
vpImage::getValue
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1592
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:184
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:152
vpTemplateTrackerMI::ApproxHessian
vpHessienApproximationType ApproxHessian
Definition: vpTemplateTrackerMI.h:73
vpTemplateTrackerMI::computeHessienNormalized
void computeHessienNormalized(vpMatrix &H)
Definition: vpTemplateTrackerMI.cpp:454
vpTemplateTrackerMI::m_d2v
std::vector< std::vector< double > > m_d2v
Definition: vpTemplateTrackerMI.h:113
vpTemplateTrackerMI::hessianComputation
vpHessienType hessianComputation
Definition: vpTemplateTrackerMI.h:72
vpTemplateTrackerMI::computeMI
void computeMI(double &MI)
Definition: vpTemplateTrackerMI.cpp:381
vpTemplateTrackerMI::m_dB
std::vector< double > m_dB
Definition: vpTemplateTrackerMI.h:111
vpTemplateTrackerMI::m_du
std::vector< double > m_du
Definition: vpTemplateTrackerMI.h:108
vpTemplateTrackerMI::computeProba
void computeProba(int &nbpoint)
Definition: vpTemplateTrackerMI.cpp:336
vpTemplateTrackerMI::vpTemplateTrackerMI
vpTemplateTrackerMI()
Default constructor.
Definition: vpTemplateTrackerMI.h:154
vpTemplateTrackerMI::computeHessien
void computeHessien(vpMatrix &H)
Definition: vpTemplateTrackerMI.cpp:414
vpTemplateTrackerMI::m_dv
std::vector< double > m_dv
Definition: vpTemplateTrackerMI.h:109
vpTemplateTrackerMI::m_d2u
std::vector< std::vector< double > > m_d2u
Definition: vpTemplateTrackerMI.h:112
vpTemplateTrackerMI::getNormalizedCost
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:242
vpTemplateTrackerMI::m_dA
std::vector< std::vector< double > > m_dA
Definition: vpTemplateTrackerMI.h:114
vpTemplateTrackerMI::setBspline
void setBspline(const vpBsplineType &newbs)
Definition: vpTemplateTrackerMI.cpp:43
vpTemplateTrackerMI::getCost
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:146
vpTemplateTrackerMI::getMI256
double getMI256(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:697
vpTemplateTrackerMI::m_A
std::vector< double > m_A
Definition: vpTemplateTrackerMI.h:110
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:74
vpTemplateTracker::Warp
vpTemplateTrackerWarp * Warp
Definition: vpTemplateTracker.h:129
vpTemplateTracker::BI
vpImage< double > BI
Definition: vpTemplateTracker.h:140
vpTemplateTracker::templateSize
unsigned int templateSize
Definition: vpTemplateTracker.h:77
vpTrackingException
Error that can be emitted by the vpTracker class and its derivatives.
Definition: vpTrackingException.h:52
vpTrackingException::notEnoughPointError
@ notEnoughPointError
Not enough point to track.
Definition: vpTrackingException.h:62