Visual Servoing Platform version 3.5.0
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 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
78}
79
81 : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0), temp(NULL),
82 Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL), dprtemp(NULL), PrtD(NULL), dPrtD(NULL),
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;
88
89 dW.resize(2, 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
113}
114
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
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
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
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
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
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
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++) {
442 Hessian[it][jt] += dprtemp[it] * dprtemp[jt] * Prt_Pt_ +
443 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
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
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
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
589double 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 } 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:304
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
unsigned int getWidth() const
Definition: vpImage.h:246
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpHessienApproximationType ApproxHessian
std::vector< std::vector< double > > m_d2u
void computeHessienNormalized(vpMatrix &H)
vpHessienType hessianComputation
std::vector< std::vector< double > > m_dA
std::vector< std::vector< double > > m_d2v
void computeMI(double &MI)
std::vector< double > m_dB
std::vector< double > m_du
void computeProba(int &nbpoint)
vpTemplateTrackerMI()
Default constructor.
void computeHessien(vpMatrix &H)
std::vector< double > m_dv
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
void setBspline(const vpBsplineType &newbs)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
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 emited by the vpTracker class and its derivates.