Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
vpTemplateTrackerMI.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Example of template tracking.
32 *
33 * Authors:
34 * Amaury Dame
35 * Aurelien Yol
36 */
37#include <visp3/core/vpException.h>
38#include <visp3/tt_mi/vpTemplateTrackerMI.h>
39#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
40
41BEGIN_VISP_NAMESPACE
42void vpTemplateTrackerMI::setBspline(const vpBsplineType &newbs)
43{
44 bspline = static_cast<int>(newbs);
45 influBspline = bspline * bspline;
46 Ncb = Nc + bspline;
47 if (Pt)
48 delete[] Pt;
49 if (Pr)
50 delete[] Pr;
51 if (Prt)
52 delete[] Prt;
53 if (dPrt)
54 delete[] dPrt;
55 if (d2Prt)
56 delete[] d2Prt;
57 if (PrtD)
58 delete[] PrtD;
59 if (dPrtD)
60 delete[] dPrtD;
61 if (PrtTout)
62 delete[] PrtTout;
63
64 Pt = new double[Ncb];
65 Pr = new double[Ncb];
66
67 Prt = new double[Ncb * Ncb];
68 dPrt = new double[Ncb * Ncb * static_cast<int>(nbParam)];
69 d2Prt = new double[Ncb * Ncb * static_cast<int>(nbParam * nbParam)];
70
71 PrtD = new double[Nc * Nc * influBspline];
72 dPrtD = new double[Nc * Nc * static_cast<int>(nbParam)*influBspline];
73 PrtTout = new double[Nc * Nc * influBspline * (1 + static_cast<int>(nbParam + nbParam * nbParam))];
74
75 hessianComputation = USE_HESSIEN_DESIRE;
76}
77
78vpTemplateTrackerMI::vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp)
79 : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0), temp(nullptr),
80 Prt(nullptr), dPrt(nullptr), Pt(nullptr), Pr(nullptr), d2Prt(nullptr), PrtTout(nullptr), dprtemp(nullptr), PrtD(nullptr), dPrtD(nullptr),
81 influBspline(0), bspline(3), Nc(8), Ncb(0), d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
82 NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
83{
84 Ncb = Nc + bspline;
85 influBspline = bspline * bspline;
86
87 dW.resize(2, nbParam);
88 H.resize(nbParam, nbParam);
89 G.resize(nbParam);
90 Hdesire.resize(nbParam, nbParam);
91 HLM.resize(nbParam, nbParam);
92 HLMdesire.resize(nbParam, nbParam);
93 dprtemp = new double[nbParam];
94 temp = new double[nbParam];
95
96 X1.resize(2);
97 X2.resize(2);
98
99 PrtD = new double[Nc * Nc * influBspline]; //(r,t)
100 dPrtD = new double[Nc * Nc * static_cast<int>(nbParam)*influBspline];
101
102 Prt = new double[Ncb * Ncb]; //(r,t)
103 Pt = new double[Ncb];
104 Pr = new double[Ncb];
105 dPrt = new double[Ncb * Ncb * static_cast<int>(nbParam)];
106 d2Prt = new double[Ncb * Ncb * static_cast<int>(nbParam * nbParam)];
107
108 PrtTout = new double[Nc * Nc * influBspline * (1 + static_cast<int>(nbParam + nbParam * nbParam))];
109
110 lambda = lambdaDep;
111}
112
113void vpTemplateTrackerMI::setNc(int nc)
114{
115 Nc = nc;
116 Ncb = Nc + bspline;
117
118 if (Pt)
119 delete[] Pt;
120 if (Pr)
121 delete[] Pr;
122 if (Prt)
123 delete[] Prt;
124 if (dPrt)
125 delete[] dPrt;
126 if (d2Prt)
127 delete[] d2Prt;
128 if (PrtD)
129 delete[] PrtD;
130 if (dPrtD)
131 delete[] dPrtD;
132 if (PrtTout)
133 delete[] PrtTout;
134
135 PrtD = new double[Nc * Nc * influBspline]; //(r,t)
136 dPrtD = new double[Nc * Nc * static_cast<int>(nbParam)*influBspline];
137 Prt = new double[Ncb * Ncb]; //(r,t)
138 dPrt = new double[Ncb * Ncb * static_cast<int>(nbParam)];
139 Pt = new double[Ncb];
140 Pr = new double[Ncb];
141 d2Prt = new double[Ncb * Ncb * static_cast<int>(nbParam * nbParam)]; //(r,t)
142 PrtTout = new double[Nc * Nc * influBspline * (1 + static_cast<int>(nbParam + nbParam * nbParam))];
143}
144
145double vpTemplateTrackerMI::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
146{
147 double MI = 0;
148 int Nbpoint = 0;
149 double IW;
150
151 unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
152 unsigned int Nc_ = static_cast<unsigned int>(Nc);
153 unsigned int influBspline_ = static_cast<unsigned int>(influBspline);
154
155 memset(Prt, 0, Ncb_ * Ncb_ * sizeof(double));
156 memset(PrtD, 0, Nc_ * Nc_ * influBspline_ * sizeof(double));
157
158 Warp->computeCoeff(tp);
159 for (unsigned int point = 0; point < templateSize; point++) {
160 X1[0] = ptTemplate[point].x;
161 X1[1] = ptTemplate[point].y;
162
163 Warp->computeDenom(X1, tp);
164 Warp->warpX(X1, X2, tp);
165 double j2 = X2[0];
166 double i2 = X2[1];
167
168 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
169 Nbpoint++;
170
171 double Tij = ptTemplate[point].val;
172 if (!blur)
173 IW = I.getValue(i2, j2);
174 else
175 IW = BI.getValue(i2, j2);
176
177 double Nc_1 = (Nc - 1.) / 255.;
178 double IW_Nc = IW * Nc_1;
179 double Tij_Nc = Tij * Nc_1;
180 int cr = static_cast<int>(IW_Nc);
181 int ct = static_cast<int>(Tij_Nc);
182 double er = IW_Nc - cr;
183 double et = Tij_Nc - ct;
184
185 // Calcul de l'histogramme joint par interpolation bilineaire
186 // (Bspline ordre 1)
187 vpTemplateTrackerMIBSpline::PutPVBsplineD(PrtD, cr, er, ct, et, Nc, 1., bspline);
188 }
189 }
190
191 ratioPixelIn = static_cast<double>(Nbpoint) / static_cast<double>(templateSize);
192
193 double *pt = PrtD;
194 for (int r = 0; r < Nc; r++)
195 for (int t = 0; t < Nc; t++) {
196 for (int i = 0; i < influBspline; i++) {
197 int r2, t2;
198 r2 = r + i / bspline;
199 t2 = t + i % bspline;
200 Prt[r2 * Ncb + t2] += *pt;
201
202 pt++;
203 }
204 }
205
206 if (Nbpoint == 0)
207 return 0;
208 for (unsigned int r = 0; r < Ncb_; r++) {
209 for (unsigned int t = 0; t < Ncb_; t++) {
210 Prt[r * Ncb_ + t] /= Nbpoint;
211 }
212 }
213 // Compute Pr, Pt
214 memset(Pr, 0, Ncb_ * sizeof(double));
215 memset(Pt, 0, Ncb_ * sizeof(double));
216 for (unsigned int r = 0; r < Ncb_; r++) {
217 for (unsigned int t = 0; t < Ncb_; t++) {
218 Pr[r] += Prt[r * Ncb_ + t];
219 Pt[r] += Prt[t * Ncb_ + r];
220 }
221 }
222
223 for (unsigned int r = 0; r < Ncb_; r++) {
224 if (std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon()) {
225 MI -= Pr[r] * log(Pr[r]);
226 }
227 if (std::fabs(Pt[r]) > std::numeric_limits<double>::epsilon()) {
228 MI -= Pt[r] * log(Pt[r]);
229 }
230 for (unsigned int t = 0; t < Ncb_; t++) {
231 unsigned int r_Ncb_t_ = r * Ncb_ + t;
232 if (std::fabs(Prt[r_Ncb_t_]) > std::numeric_limits<double>::epsilon()) {
233 MI += Prt[r_Ncb_t_] * log(Prt[r_Ncb_t_]);
234 }
235 }
236 }
237
238 return -MI;
239}
240
241double vpTemplateTrackerMI::getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp)
242{
243 // Attention, cette version calculee de la NMI ne pourra pas atteindre le
244 // maximum de 2. Ceci est du au fait que par defaut, l'image est floutee dans
245 // vpTemplateTracker::initTracking()
246
247 double MI = 0;
248 double Nbpoint = 0;
249 double IW;
250
251 double Pr_[256];
252 double Pt_[256];
253 double Prt_[256][256];
254
255 memset(Pr_, 0, 256 * sizeof(double));
256 memset(Pt_, 0, 256 * sizeof(double));
257 memset(Prt_, 0, 256 * 256 * sizeof(double));
258
259 for (unsigned int point = 0; point < templateSize; point++) {
260 X1[0] = ptTemplate[point].x;
261 X1[1] = ptTemplate[point].y;
262
263 Warp->computeDenom(X1, tp);
264 Warp->warpX(X1, X2, tp);
265 double j2 = X2[0];
266 double i2 = X2[1];
267
268 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
269 Nbpoint++;
270 double Tij = ptTemplate[point].val;
271 int Tij_ = static_cast<int>(Tij);
272 if (!blur) {
273 IW = I[static_cast<int>(i2)][static_cast<int>(j2)];
274 }
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
312vpTemplateTrackerMI::~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
336void 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
381void vpTemplateTrackerMI::computeMI(double &MI)
382{
383 unsigned int Ncb_ = static_cast<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
414void 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
454void 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
544void 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
571void 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
587double 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_ = static_cast<int>(tNcb);
639 int tinfluBspline_ = static_cast<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 }
660 else {
661 for (unsigned int r = 0; r < tNcb; r++)
662 for (unsigned int t = 0; t < tNcb; t++)
663 tPrt[r * tNcb + t] = tPrt[r * tNcb + t] / Nbpoint;
664 // calcul Pr;
665 memset(tPr, 0, tNcb * sizeof(double));
666 for (unsigned int r = 0; r < tNcb; r++) {
667 for (unsigned int t = 0; t < tNcb; t++)
668 tPr[r] += tPrt[r * tNcb + t];
669 }
670
671 // calcul Pt;
672 memset(tPt, 0, static_cast<size_t>(tNcb * sizeof(double)));
673 for (unsigned int t = 0; t < tNcb; t++) {
674 for (unsigned int r = 0; r < tNcb; r++)
675 tPt[t] += tPrt[r * tNcb + t];
676 }
677 for (unsigned int r = 0; r < tNcb; r++)
678 if (std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
679 MI -= tPr[r] * log(tPr[r]);
680
681 for (unsigned int t = 0; t < tNcb; t++)
682 if (std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
683 MI -= tPt[t] * log(tPt[t]);
684
685 for (unsigned int r = 0; r < tNcb; r++)
686 for (unsigned int t = 0; t < tNcb; t++)
687 if (std::fabs(tPrt[r * tNcb + t]) > std::numeric_limits<double>::epsilon())
688 MI += tPrt[r * tNcb + t] * log(tPrt[r * tNcb + t]);
689 }
690 delete[] tPrtD;
691 delete[] tPrt;
692 delete[] tPr;
693 delete[] tPt;
694
695 return MI;
696}
697
698double vpTemplateTrackerMI::getMI256(const vpImage<unsigned char> &I, const vpColVector &tp)
699{
700 vpMatrix Prt256(256, 256);
701 Prt256 = 0;
702 vpColVector Pr256(256);
703 Pr256 = 0;
704 vpColVector Pt256(256);
705 Pt256 = 0;
706
707 volatile int Nbpoint = 0;
708 unsigned int Tij, IW;
709
710 vpImage<double> GaussI;
711 if (blur)
712 vpImageFilter::filter(I, GaussI, fgG, taillef);
713
714 Warp->computeCoeff(tp);
715 for (unsigned int point = 0; point < templateSize; point++) {
716 X1[0] = ptTemplate[point].x;
717 X1[1] = ptTemplate[point].y;
718
719 Warp->computeDenom(X1, tp);
720 Warp->warpX(X1, X2, tp);
721 double j2 = X2[0];
722 double i2 = X2[1];
723
724 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth()) - 1) {
725 Nbpoint++;
726
727 Tij = static_cast<unsigned int>(ptTemplate[point].val);
728 if (!blur)
729 IW = static_cast<unsigned int>(I.getValue(i2, j2));
730 else
731 IW = static_cast<unsigned int>(GaussI.getValue(i2, j2));
732
733 Prt256[Tij][IW]++;
734 Pr256[Tij]++;
735 Pt256[IW]++;
736 }
737 }
738
739 if (Nbpoint == 0) {
740 throw(vpException(vpException::divideByZeroError, "Cannot get MI; number of points = 0"));
741 }
742 Prt256 = Prt256 / Nbpoint;
743 Pr256 = Pr256 / Nbpoint;
744 Pt256 = Pt256 / Nbpoint;
745
746 double MI = 0;
747
748 for (unsigned int t = 0; t < 256; t++) {
749 for (unsigned int r = 0; r < 256; r++) {
750 if (std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
751 MI += Prt256[r][t] * log(Prt256[r][t]);
752 }
753 if (std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
754 MI += -Pt256[t] * log(Pt256[t]);
755 if (std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
756 MI += -Pr256[t] * log(Pr256[t]);
757 }
758 return MI;
759}
760END_VISP_NAMESPACE
vpColVector
Implementation of column vector and the associated operations.
Definition vpColVector.h:191
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:715
vpImage
Definition of the vpImage class member functions.
Definition vpImage.h:131
vpImage::getValue
Type getValue(unsigned int i, unsigned int j) const
Definition vpImage_getters.h:51
vpMatrix
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:175
vpTemplateTrackerMI::ApproxHessian
vpHessienApproximationType ApproxHessian
Definition vpTemplateTrackerMI.h:76
vpTemplateTrackerMI::d2Ix
vpImage< double > d2Ix
Definition vpTemplateTrackerMI.h:97
vpTemplateTrackerMI::computeHessienNormalized
void computeHessienNormalized(vpMatrix &H)
Definition vpTemplateTrackerMI.cpp:454
vpTemplateTrackerMI::m_d2v
std::vector< std::vector< double > > m_d2v
Definition vpTemplateTrackerMI.h:116
vpTemplateTrackerMI::hessianComputation
vpHessienType hessianComputation
Definition vpTemplateTrackerMI.h:75
vpTemplateTrackerMI::getCost
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp) VP_OVERRIDE
Definition vpTemplateTrackerMI.cpp:145
vpTemplateTrackerMI::m_dB
std::vector< double > m_dB
Definition vpTemplateTrackerMI.h:114
vpTemplateTrackerMI::m_du
std::vector< double > m_du
Definition vpTemplateTrackerMI.h:111
vpTemplateTrackerMI::computeProba
void computeProba(int &nbpoint)
Definition vpTemplateTrackerMI.cpp:336
vpTemplateTrackerMI::vpTemplateTrackerMI
vpTemplateTrackerMI()
Default constructor.
Definition vpTemplateTrackerMI.h:159
vpTemplateTrackerMI::~vpTemplateTrackerMI
virtual ~vpTemplateTrackerMI() VP_OVERRIDE
Definition vpTemplateTrackerMI.cpp:312
vpTemplateTrackerMI::computeHessien
void computeHessien(vpMatrix &H)
Definition vpTemplateTrackerMI.cpp:414
vpTemplateTrackerMI::m_dv
std::vector< double > m_dv
Definition vpTemplateTrackerMI.h:112
vpTemplateTrackerMI::m_d2u
std::vector< std::vector< double > > m_d2u
Definition vpTemplateTrackerMI.h:115
vpTemplateTrackerMI::d2Ixy
vpImage< double > d2Ixy
Definition vpTemplateTrackerMI.h:99
vpTemplateTrackerMI::getNormalizedCost
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition vpTemplateTrackerMI.cpp:241
vpTemplateTrackerMI::m_dA
std::vector< std::vector< double > > m_dA
Definition vpTemplateTrackerMI.h:117
vpTemplateTrackerMI::d2Iy
vpImage< double > d2Iy
Definition vpTemplateTrackerMI.h:98
vpTemplateTrackerMI::getMI256
double getMI256(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition vpTemplateTrackerMI.cpp:698
vpTemplateTrackerMI::m_A
std::vector< double > m_A
Definition vpTemplateTrackerMI.h:113
vpTemplateTrackerMI::setBspline
void setBspline(const vpBsplineType &newbs)
Definition vpTemplateTrackerMI.cpp:42
vpTemplateTracker::vpTemplateTracker
vpTemplateTracker()
Default constructor.
Definition vpTemplateTracker.h:154
vpTemplateTracker::ptTemplate
vpTemplateTrackerPoint * ptTemplate
Definition vpTemplateTracker.h:81
vpTemplateTracker::Warp
vpTemplateTrackerWarp * Warp
Definition vpTemplateTracker.h:136
vpTemplateTracker::BI
vpImage< double > BI
Definition vpTemplateTracker.h:147
vpTemplateTracker::templateSize
unsigned int templateSize
Definition vpTemplateTracker.h:84
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
BEGIN_VISP_NAMESPACE
Definition vpMbtDistanceCircle.cpp:55