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