Visual Servoing Platform  version 3.6.1 under development (2024-04-19)
vpTemplateTrackerMIBSpline.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  * Template tracker.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  *
38 *****************************************************************************/
39 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
40 
41 #ifndef DOXYGEN_SHOULD_SKIP_THIS
42 
43 void vpTemplateTrackerMIBSpline::PutPVBsplineD(double *Prt, int cr, double er, int ct, double et, int Nc, double val,
44  const int &degre)
45 {
46  switch (degre) {
47  case 4:
48  PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
49  break;
50  default:
51  PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);
52  }
53 }
54 
55 void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
56 {
57  int sr = 0;
58  int st = 0;
59  if (er > 0.5) {
60  sr = 1;
61  er = er - 1;
62  }
63  if (et > 0.5) {
64  st = 1;
65  et = et - 1;
66  }
67  double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9];
68  if (std::fabs(val - 1.) > std::numeric_limits<double>::epsilon()) {
69  for (int ir = -1; ir <= 1; ir++) {
70  double Bspline3_diff_r_ = Bspline3(ir - er);
71  for (int it = -1; it <= 1; it++) {
72  *pt++ += Bspline3_diff_r_ * Bspline3(it - et) * val;
73  }
74  }
75  } else {
76  for (int ir = -1; ir <= 1; ir++) {
77  double Bspline3_diff_r_ = Bspline3(ir - er);
78  for (int it = -1; it <= 1; it++) {
79  *pt++ += Bspline3_diff_r_ * Bspline3(it - et);
80  }
81  }
82  }
83 }
84 
85 void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
86 {
87  double Bti[4];
88 
89  double *ptBti = &Bti[0];
90  for (int it = -1; it <= 2; it++) {
91  *ptBti++ = Bspline4i(it - et, it);
92  // pt++;
93  }
94  double *pt = &Prt[(cr * Nc + ct) * 16];
95  if (std::fabs(val - 1.) > std::numeric_limits<double>::epsilon()) {
96  for (int ir = -1; ir <= 2; ir++) {
97  double Br = Bspline4i(ir - er, ir);
98  ptBti = &Bti[0];
99  for (int it = -1; it <= 2; it++) {
100  *pt++ += Br * (*ptBti++) * val;
101  }
102  }
103  } else {
104  for (int ir = -1; ir <= 2; ir++) {
105  double Br = Bspline4i(ir - er, ir);
106  ptBti = &Bti[0];
107  for (int it = -1; it <= 2; it++) {
108  *pt++ += Br * (*ptBti++);
109  }
110  }
111  }
112 }
113 
114 double vpTemplateTrackerMIBSpline::Bspline3(double diff)
115 {
116  double aDiff = std::fabs(diff);
117 
118  if (aDiff < 1.5) {
119  if (aDiff < 0.5) {
120  return (-(aDiff * aDiff) + 0.75);
121  }
122  double tmp_ = 1.5 - aDiff;
123  return (0.5 * tmp_ * tmp_);
124  }
125 
126  return 0;
127 }
128 
129 double vpTemplateTrackerMIBSpline::Bspline4i(double diff, int &interv)
130 {
131  switch (interv) {
132  case 2:
133  case -1: {
134  double tmp_ = 2. + diff;
135  return (tmp_ * tmp_ * tmp_ / 6.);
136  }
137  case 0: {
138  double diff2_ = diff * diff;
139  return (-diff2_ * diff / 2. - diff2_ + 4. / 6.);
140  }
141  case 1: {
142  double diff2_ = diff * diff;
143  return (diff2_ * diff / 2. - diff2_ + 4. / 6.);
144  }
145  default:
146  return 0;
147  }
148 }
149 
150 double vpTemplateTrackerMIBSpline::dBspline3(double diff)
151 {
152  if ((diff > -1.5) && (diff <= -0.5))
153  return diff + 1.5;
154  else if ((diff > -0.5) && (diff <= 0.5))
155  return -2. * diff;
156  else if ((diff > 0.5) && (diff <= 1.5))
157  return diff - 1.5;
158 
159  return 0;
160 }
161 
162 double vpTemplateTrackerMIBSpline::dBspline4(double diff)
163 {
164  if ((diff > -2.) && (diff <= -1.)) {
165  double diff_2_ = diff + 2.;
166  return (diff_2_ * diff_2_ * 0.5);
167  } else if ((diff > -1.) && (diff <= 0.)) {
168  return -1.5 * diff * diff - 2. * diff;
169  } else if ((diff > 0.) && (diff <= 1.)) {
170  return 1.5 * diff * diff - 2. * diff;
171  } else if ((diff > 1.) && (diff <= 2.)) {
172  double diff_2_ = diff - 2.;
173  return (-0.5 * diff_2_ * diff_2_);
174  } else {
175  return 0;
176  }
177 }
178 
179 double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
180 {
181  if ((diff > -1.5) && (diff <= -0.5))
182  return 1.;
183  else if ((diff > -0.5) && (diff <= 0.5))
184  return -2.;
185  else if ((diff > 0.5) && (diff <= 1.5))
186  return 1.;
187  else
188  return 0;
189 }
190 
191 double vpTemplateTrackerMIBSpline::d2Bspline4(double diff)
192 {
193  if ((diff > -2.) && (diff <= -1.))
194  return (diff + 2.);
195  else if ((diff > -1.) && (diff <= 0.))
196  return -3. * diff - 2.;
197  else if ((diff > 0.) && (diff <= 1.))
198  return 3. * diff - 2.;
199  else if ((diff > 1.) && (diff <= 2.))
200  return -(diff - 2.);
201  else
202  return 0;
203 }
204 
205 void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et, int Nc,
206  double *val, unsigned int &NbParam, int &degree)
207 {
208  switch (degree) {
209  case 4:
210  PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
211  break;
212  default:
213  PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
214  }
215 }
216 
217 void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct,
218  double &et, int Ncb, double *val, unsigned int &NbParam, int &degree)
219 {
220  switch (degree) {
221  case 4:
222  PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
223  break;
224  default:
225  PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
226  }
227 }
228 
229 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et, int Nc,
230  double *val, unsigned int &NbParam)
231 {
232  short int sr = 0;
233  short int st = 0;
234  if (er > 0.5) {
235  sr = 1;
236  er = er - 1;
237  }
238  if (et > 0.5) {
239  st = 1;
240  et = et - 1;
241  }
242 
243  double Bti[3];
244  double dBti[3];
245  double d2Bti[3];
246 
247  double *ptBti = &Bti[0];
248  double *ptdBti = &dBti[0];
249  double *ptd2Bti = &d2Bti[0];
250 
251  for (short int it = 1; it >= -1; it--) {
252  *ptBti++ = Bspline3(it + et);
253  *ptdBti++ = dBspline3(it + et);
254  *ptd2Bti++ = d2Bspline3(it + et);
255  }
256 
257  double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + (int)(NbParam + NbParam * NbParam))];
258  for (short int ir = -1; ir <= 1; ++ir) {
259  double Br = Bspline3(-ir + er);
260 
261  for (short unsigned int it = 0; it <= 2; ++it) {
262  *pt++ += Br * (Bti[it]);
263 
264  double v1 = Br * (dBti[it]);
265  for (short unsigned int ip = 0; ip < NbParam; ++ip) {
266  *pt++ -= v1 * val[ip];
267  double v2 = Br * (d2Bti[it]) * val[ip];
268  for (short unsigned int ip2 = 0; ip2 < NbParam; ++ip2)
269  *pt++ += v2 * val[ip2];
270  }
271  }
272  }
273 }
274 
275 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct,
276  double &et, int Ncb, double *val, unsigned int &NbParam)
277 {
278  short int sr = 0;
279  short int st = 0;
280  if (er > 0.5) {
281  sr = 1;
282  er = er - 1;
283  }
284  if (et > 0.5) {
285  st = 1;
286  et = et - 1;
287  }
288 
289  double Bti[3];
290  double dBti[3];
291  double d2Bti[3];
292 
293  double *ptBti = &Bti[0];
294  double *ptdBti = &dBti[0];
295  double *ptd2Bti = &d2Bti[0];
296 
297  for (short int it = 1; it >= -1; it--) {
298  *ptBti++ = Bspline3(it + et);
299  *ptdBti++ = dBspline3(it + et);
300  *ptd2Bti++ = d2Bspline3(it + et);
301  }
302 
303  int NbParam_ = (int)NbParam;
304  for (short int ir = -1; ir <= 1; ++ir) {
305  double Br = Bspline3(-ir + er);
306  short int irInd = ir + 1;
307  short int ind = (cr + sr + irInd) * Ncb;
308  for (short int it = 0; it <= 2; ++it) {
309  Prt[ind + (ct + st + it)] += Br * (Bti[it]);
310 
311  double v1 = Br * (dBti[it]);
312  int ind1 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_;
313  for (int ip = 0; ip < NbParam_; ++ip) {
314  dPrt[ind1 + ip] -= v1 * val[ip];
315  double v2 = Br * (d2Bti[it]) * val[ip];
316  int ind2 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_ * NbParam_ + ip * NbParam_;
317  for (short int ip2 = 0; ip2 < NbParam_; ++ip2)
318  d2Prt[ind2 + ip2] += v2 * val[ip2];
319  }
320  }
321  }
322 }
323 
324 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size)
325 {
326 #define LSIZE 12
327 
328  double *bt0 = &bt[0];
329  if (er > 0.5) {
330  er = er - 1.0;
331  }
332 
333  for (int ir = -1; ir <= 1; ++ir) {
334  double Br = Bspline3(-ir + er);
335  const double *btend = bt0 + size;
336  bt = bt0;
337 
338  if (size >= LSIZE) {
339  btend -= LSIZE - 1;
340  for (; bt < btend; bt += LSIZE) {
341  *Prt++ += Br * bt[0];
342  *Prt++ += Br * bt[1];
343  *Prt++ += Br * bt[2];
344  *Prt++ += Br * bt[3];
345  *Prt++ += Br * bt[4];
346  *Prt++ += Br * bt[5];
347  *Prt++ += Br * bt[6];
348  *Prt++ += Br * bt[7];
349  *Prt++ += Br * bt[8];
350  *Prt++ += Br * bt[9];
351  *Prt++ += Br * bt[10];
352  *Prt++ += Br * bt[11];
353  }
354  btend += LSIZE - 1;
355  }
356  for (; bt < btend; *Prt++ += Br * *bt++) {
357  };
358  }
359 #undef LSIZE
360 }
361 
362 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et, int Nc,
363  double *val, unsigned int &NbParam)
364 {
365  double Bti[4];
366  double dBti[4];
367  double d2Bti[4];
368 
369  double *ptBti = &Bti[0];
370  double *ptdBti = &dBti[0];
371  double *ptd2Bti = &d2Bti[0];
372  for (char it = -1; it <= 2; it++) {
373  *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
374  *ptdBti++ = dBspline4(-it + et);
375  *ptd2Bti++ = d2Bspline4(-it + et);
376  }
377 
378  int NbParam_ = (int)NbParam;
379 
380  double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)];
381  for (char ir = -1; ir <= 2; ir++) {
382  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
383  ptBti = &Bti[0];
384  ptdBti = &dBti[0];
385  ptd2Bti = &d2Bti[0];
386  for (char it = -1; it <= 2; it++) {
387  double Br_ptBti_ = Br * (*ptBti);
388  double Br_ptdBti_ = Br * (*ptdBti);
389  double Br_ptd2Bti_ = Br * (*ptd2Bti);
390  *pt++ += Br_ptBti_;
391  for (short int ip = 0; ip < NbParam_; ip++) {
392  *pt++ -= Br_ptdBti_ * val[ip];
393  for (short int ip2 = 0; ip2 < NbParam_; ip2++) {
394  *pt++ += Br_ptd2Bti_ * val[ip] * val[ip2];
395  }
396  }
397  ptBti++;
398  ptdBti++;
399  ptd2Bti++;
400  }
401  }
402 }
403 
404 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct,
405  double et, int Ncb, double *val, unsigned int &NbParam)
406 {
407  double Bti[4];
408  double dBti[4];
409  double d2Bti[4];
410 
411  for (size_t i = 0; i < 4; ++i) {
412  Bti[i] = 0.;
413  dBti[i] = 0.;
414  d2Bti[i] = 0.;
415  }
416 
417  double *ptBti = &Bti[0];
418  double *ptdBti = &dBti[0];
419  double *ptd2Bti = &d2Bti[0];
420  for (char it = -1; it <= 2; it++) {
421  *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
422  *ptdBti++ = dBspline4(-it + et);
423  *ptd2Bti++ = d2Bspline4(-it + et);
424  }
425 
426  int NbParam_ = (int)NbParam;
427 
428  for (int ir = -1; ir <= 2; ir++) {
429  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
430  int irInd = ir + 1;
431  int ind = (cr + irInd) * Ncb + ct;
432 
433  ptBti = &Bti[0];
434  ptdBti = &dBti[0];
435  ptd2Bti = &d2Bti[0];
436  for (int it = -1; it <= 2; it++) {
437  Prt[ind + it] += Br * *ptBti;
438  int ind1 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_;
439  int ind2 = ind1 * NbParam_;
440  double Br_ptdBti_ = Br * (*ptdBti);
441  double Br_ptd2Bti_ = Br * (*ptd2Bti);
442  for (int ip = 0; ip < NbParam_; ip++) {
443  dPrt[ind1 + ip] -= Br_ptdBti_ * val[ip];
444  int ind3 = ind2 + ip * NbParam_;
445  for (int ip2 = 0; ip2 < NbParam_; ip2++)
446  d2Prt[ind3 + ip2] += Br_ptd2Bti_ * val[ip] * val[ip2];
447  }
448  ptBti++;
449  ptdBti++;
450  ptd2Bti++;
451  }
452  }
453 }
454 
455 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, double *bt, unsigned int size)
456 {
457 #define LSIZE 12
458  double *bt0 = &bt[0];
459 
460  for (int ir = -1; ir <= 2; ++ir) {
461  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
462  const double *btend = bt0 + size;
463  bt = bt0;
464 
465  if (size >= LSIZE) {
466  btend -= LSIZE - 1;
467  for (; bt < btend; bt += LSIZE) {
468  *Prt++ += Br * bt[0];
469  *Prt++ += Br * bt[1];
470  *Prt++ += Br * bt[2];
471  *Prt++ += Br * bt[3];
472  *Prt++ += Br * bt[4];
473  *Prt++ += Br * bt[5];
474  *Prt++ += Br * bt[6];
475  *Prt++ += Br * bt[7];
476  *Prt++ += Br * bt[8];
477  *Prt++ += Br * bt[9];
478  *Prt++ += Br * bt[10];
479  *Prt++ += Br * bt[11];
480  }
481  btend += LSIZE - 1;
482  }
483  for (; bt < btend; *Prt++ += Br * *bt++) {
484  };
485  }
486 #undef LSIZE
487 }
488 
489 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
490  double *val, unsigned int &NbParam, int &degree)
491 {
492  switch (degree) {
493  case 4:
494  PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
495  break;
496  default:
497  PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
498  }
499 }
500 
501 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
502  double &et, int &Ncb, double *val, unsigned int &NbParam,
503  int &degree)
504 {
505  switch (degree) {
506  case 4:
507  PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
508  break;
509  default:
510  PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
511  }
512 }
513 
514 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,
515  int &Nc, double *val, unsigned int &NbParam)
516 {
517  int sr = 0;
518  int st = 0;
519  if (er > 0.5) {
520  sr = 1;
521  er = er - 1;
522  }
523  if (et > 0.5) {
524  st = 1;
525  et = et - 1;
526  }
527 
528  double Bti[3];
529  double dBti[3];
530 
531  double *ptBti = &Bti[0];
532  double *ptdBti = &dBti[0];
533  for (char it = -1; it <= 1; it++) {
534  *ptBti++ = Bspline3(-it + et);
535  *ptdBti++ = dBspline3(-it + et);
536  }
537 
538  int NbParam_ = (int)NbParam;
539 
540  double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_ + NbParam_ * NbParam_)];
541  for (char ir = -1; ir <= 1; ir++) {
542  double Br = Bspline3(-ir + er);
543  ptBti = &Bti[0];
544  ptdBti = &dBti[0];
545  for (char it = -1; it <= 1; it++) {
546  *pt++ += Br * *ptBti;
547  double Br_ptdBti_ = Br * (*ptdBti);
548  for (unsigned int ip = 0; ip < NbParam; ip++) {
549  *pt++ -= Br_ptdBti_ * val[ip];
550  pt += NbParam; // Modif AY
551  }
552  // pt=pt+NbParam*NbParam; // Modif AY
553  ptBti++;
554  ptdBti++;
555  }
556  }
557 }
558 
559 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
560  double &et, int &Ncb, double *val, unsigned int &NbParam)
561 {
562  int sr = 0;
563  int st = 0;
564  if (er > 0.5) {
565  sr = 1;
566  er = er - 1;
567  }
568  if (et > 0.5) {
569  st = 1;
570  et = et - 1;
571  }
572 
573  double Bti[3];
574  double dBti[3];
575 
576  double *ptBti = &Bti[0];
577  double *ptdBti = &dBti[0];
578  for (char it = -1; it <= 1; it++) {
579  *ptBti++ = Bspline3(-it + et);
580  *ptdBti++ = dBspline3(-it + et);
581  }
582 
583  int NbParam_ = static_cast<int>(NbParam);
584  int ct_st_ = ct + st;
585  int cr_sr_ = cr + sr;
586 
587  for (char ir = -1; ir <= 1; ir++) {
588  double Br = Bspline3(-ir + er);
589 
590  int irInd = ir + 1;
591  int ind = (cr_sr_ + irInd) * Ncb;
592  ptBti = &Bti[0];
593  ptdBti = &dBti[0];
594 
595  double Br_ptBti_ = Br * (*ptBti);
596  double Br_ptdBti_ = Br * (*ptdBti);
597  for (char it = -1; it <= 1; it++) {
598  Prt[ind + (ct_st_ + it)] += Br_ptBti_;
599  int ind1 = (ind + (ct_st_ + it)) * NbParam_;
600  for (short int ip = 0; ip < NbParam_; ip++) {
601  dPrt[ind1 + ip] -= Br_ptdBti_ * val[ip];
602  }
603  ptBti++;
604  ptdBti++;
605  }
606  }
607 }
608 
609 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,
610  int &Nc, double *val, unsigned int &NbParam)
611 {
612  double Bti[4] = {0, 0, 0, 0};
613  double dBti[4] = {0, 0, 0, 0};
614 
615  for (char it = -1; it <= 2; it++) {
616  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
617  dBti[it + 1] = dBspline4(-it + et);
618  }
619 
620  int NbParam_ = (int)NbParam;
621 
622  double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)];
623  for (int ir = -1; ir <= 2; ir++) {
624  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
625  for (int it = 0; it <= 3; it++) {
626  (*pt++) += Br * Bti[it];
627 
628  double Br_dBti_ = Br * dBti[it];
629  for (int ip = 0; ip < NbParam_; ip++) {
630  (*pt++) -= Br_dBti_ * val[ip];
631  pt += NbParam_; // Modif AY
632  }
633  // pt=pt+NbParam*NbParam; // Modif AY
634  }
635  }
636 }
637 
638 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
639  double &et, int &Ncb, double *val, unsigned int &NbParam)
640 {
641  double Bti[4] = {0, 0, 0, 0};
642  double dBti[4] = {0, 0, 0, 0};
643 
644  for (char it = -1; it <= 2; it++) {
645  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
646  dBti[it + 1] = dBspline4(-it + et);
647  }
648 
649  int NbParam_ = static_cast<int>(NbParam);
650 
651  for (int ir = -1; ir <= 2; ir++) {
652  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
653  int irInd = ir + 1;
654  int ind = (cr + irInd) * Ncb;
655  int ind1 = ind + ct;
656 
657  for (int it = 0; it <= 3; it++) {
658  Prt[ind1 + it] += Br * Bti[it];
659  int ind2 = (ind + (ct + it)) * NbParam_;
660 
661  double Br_dBti_ = Br * dBti[it];
662  for (int ip = 0; ip < NbParam_; ip++) {
663  dPrt[ind2 + ip] -= Br_dBti_ * val[ip];
664  }
665  }
666  }
667 }
668 
669 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
670  int &Nc, unsigned int &NbParam, int &degree)
671 {
672  switch (degree) {
673  case 4:
674  PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
675  break;
676  default:
677  PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
678  }
679 }
680 
681 void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
682  int &Nc, unsigned int &NbParam)
683 {
684  int sr = 0;
685  int st = 0;
686  if (er > 0.5) {
687  sr = 1;
688  er = er - 1;
689  }
690  if (et > 0.5) {
691  st = 1;
692  et = et - 1;
693  }
694 
695  double Bti[3] = {0, 0, 0};
696 
697  for (char it = -1; it <= 1; it++) {
698  Bti[it + 1] = Bspline3(-it + et);
699  }
700 
701  int NbParam_ = static_cast<int>(NbParam);
702  int NbParam_val = NbParam_ + NbParam_ * NbParam_;
703 
704  double *pt = &PrtTout[(unsigned int)(((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_val))];
705  for (int ir = -1; ir <= 1; ir++) {
706  double Br = Bspline3(-ir + er);
707  for (int it = 0; it <= 2; it++) {
708  (*pt++) += Br * Bti[it];
709  pt += NbParam_val;
710  }
711  }
712 }
713 void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
714  int &Nc, unsigned int &NbParam)
715 {
716  double Bti[4] = {0, 0, 0, 0};
717 
718  for (char it = -1; it <= 2; it++) {
719  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
720  }
721 
722  int NbParam_ = static_cast<int>(NbParam);
723  int NbParam_val = NbParam_ + NbParam_ * NbParam_;
724  double *pt = &PrtTout[(unsigned int)((cr * Nc + ct) * 16 * (1 + NbParam_val))];
725  for (int ir = -1; ir <= 2; ir++) {
726  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
727  for (int it = 0; it <= 3; it++) {
728  (*pt++) += Br * Bti[it];
729  pt += NbParam_val;
730  }
731  }
732 }
733 
734 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb,
735  unsigned int &NbParam, int &degree)
736 {
737  switch (degree) {
738  case 4:
739  PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
740  break;
741  default:
742  PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
743  }
744 }
745 
746 void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
747 {
748 
749  int sr = 0;
750  int st = 0;
751  if (er > 0.5) {
752  sr = 1;
753  er = er - 1;
754  }
755  if (et > 0.5) {
756  st = 1;
757  et = et - 1;
758  }
759 
760  double Bti[3] = {0, 0, 0};
761 
762  for (char it = -1; it <= 1; it++) {
763  Bti[it + 1] = Bspline3(-it + et);
764  }
765 
766  int ct_st_ = ct + st;
767  int cr_sr_ = cr + sr;
768  for (int ir = -1; ir <= 1; ir++) {
769  double Br = Bspline3(-ir + er);
770 
771  int irInd = ir + 1;
772  int ind = (cr_sr_ + irInd) * Ncb;
773  for (int it = 0; it <= 2; it++) {
774  Prt[ind + (ct_st_ + it)] += Br * Bti[it];
775  }
776  }
777 }
778 
779 void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
780 {
781  double Bti[4] = {0, 0, 0, 0};
782 
783  for (char it = -1; it <= 2; it++) {
784  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
785  }
786 
787  for (int ir = -1; ir <= 2; ir++) {
788  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
789  int irInd = ir + 1;
790  int ind = (cr + irInd) * Ncb + ct;
791 
792  for (int it = 0; it <= 3; it++) {
793  Prt[ind + it] += Br * Bti[it];
794  }
795  }
796 }
797 
798 void vpTemplateTrackerMIBSpline::computeProbabilities(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
799  double *dW, unsigned int &NbParam, int &bspline,
800  vpTemplateTrackerMI::vpHessienApproximationType &approx,
801  bool use_hessien_des)
802 {
803  if (approx == vpTemplateTrackerMI::HESSIAN_NONSECOND || use_hessien_des) {
804  if (bspline == 3)
805  PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
806  else
807  PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
808  } else {
809  if (bspline == 3)
810  PutTotPVBspline3(Prt, cr, er, ct, et, Nc, dW, NbParam);
811  else
812  PutTotPVBspline4(Prt, cr, er, ct, et, Nc, dW, NbParam);
813  }
814 }
815 
816 #endif