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