Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
43void 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
55void 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
85void 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
114double 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
129double 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
150double 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
162double 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
179double 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
191double 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
205void 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
217void 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
229void 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
275void 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
324void 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
362void 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
404void 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 double *ptBti = &Bti[0];
412 double *ptdBti = &dBti[0];
413 double *ptd2Bti = &d2Bti[0];
414 for (char it = -1; it <= 2; it++) {
415 *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
416 *ptdBti++ = dBspline4(-it + et);
417 *ptd2Bti++ = d2Bspline4(-it + et);
418 }
419
420 int NbParam_ = (int)NbParam;
421
422 for (int ir = -1; ir <= 2; ir++) {
423 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
424 int irInd = ir + 1;
425 int ind = (cr + irInd) * Ncb + ct;
426
427 ptBti = &Bti[0];
428 ptdBti = &dBti[0];
429 ptd2Bti = &d2Bti[0];
430 for (int it = -1; it <= 2; it++) {
431 Prt[ind + it] += Br * *ptBti;
432 int ind1 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_;
433 int ind2 = ind1 * NbParam_;
434 double Br_ptdBti_ = Br * (*ptdBti);
435 double Br_ptd2Bti_ = Br * (*ptd2Bti);
436 for (int ip = 0; ip < NbParam_; ip++) {
437 dPrt[ind1 + ip] -= Br_ptdBti_ * val[ip];
438 int ind3 = ind2 + ip * NbParam_;
439 for (int ip2 = 0; ip2 < NbParam_; ip2++)
440 d2Prt[ind3 + ip2] += Br_ptd2Bti_ * val[ip] * val[ip2];
441 }
442 ptBti++;
443 ptdBti++;
444 ptd2Bti++;
445 }
446 }
447}
448
449void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, double *bt, unsigned int size)
450{
451#define LSIZE 12
452 double *bt0 = &bt[0];
453
454 for (int ir = -1; ir <= 2; ++ir) {
455 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
456 const double *btend = bt0 + size;
457 bt = bt0;
458
459 if (size >= LSIZE) {
460 btend -= LSIZE - 1;
461 for (; bt < btend; bt += LSIZE) {
462 *Prt++ += Br * bt[0];
463 *Prt++ += Br * bt[1];
464 *Prt++ += Br * bt[2];
465 *Prt++ += Br * bt[3];
466 *Prt++ += Br * bt[4];
467 *Prt++ += Br * bt[5];
468 *Prt++ += Br * bt[6];
469 *Prt++ += Br * bt[7];
470 *Prt++ += Br * bt[8];
471 *Prt++ += Br * bt[9];
472 *Prt++ += Br * bt[10];
473 *Prt++ += Br * bt[11];
474 }
475 btend += LSIZE - 1;
476 }
477 for (; bt < btend; *Prt++ += Br * *bt++) {
478 };
479 }
480#undef LSIZE
481}
482
483void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
484 double *val, unsigned int &NbParam, int &degree)
485{
486 switch (degree) {
487 case 4:
488 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
489 break;
490 default:
491 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
492 }
493}
494
495void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
496 double &et, int &Ncb, double *val, unsigned int &NbParam,
497 int &degree)
498{
499 switch (degree) {
500 case 4:
501 PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
502 break;
503 default:
504 PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
505 }
506}
507
508void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,
509 int &Nc, double *val, unsigned int &NbParam)
510{
511 int sr = 0;
512 int st = 0;
513 if (er > 0.5) {
514 sr = 1;
515 er = er - 1;
516 }
517 if (et > 0.5) {
518 st = 1;
519 et = et - 1;
520 }
521
522 double Bti[3];
523 double dBti[3];
524
525 double *ptBti = &Bti[0];
526 double *ptdBti = &dBti[0];
527 for (char it = -1; it <= 1; it++) {
528 *ptBti++ = Bspline3(-it + et);
529 *ptdBti++ = dBspline3(-it + et);
530 }
531
532 int NbParam_ = (int)NbParam;
533
534 double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_ + NbParam_ * NbParam_)];
535 for (char ir = -1; ir <= 1; ir++) {
536 double Br = Bspline3(-ir + er);
537 ptBti = &Bti[0];
538 ptdBti = &dBti[0];
539 for (char it = -1; it <= 1; it++) {
540 *pt++ += Br * *ptBti;
541 double Br_ptdBti_ = Br * (*ptdBti);
542 for (unsigned int ip = 0; ip < NbParam; ip++) {
543 *pt++ -= Br_ptdBti_ * val[ip];
544 pt += NbParam; // Modif AY
545 }
546 // pt=pt+NbParam*NbParam; // Modif AY
547 ptBti++;
548 ptdBti++;
549 }
550 }
551}
552
553void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
554 double &et, int &Ncb, double *val, unsigned int &NbParam)
555{
556 int sr = 0;
557 int st = 0;
558 if (er > 0.5) {
559 sr = 1;
560 er = er - 1;
561 }
562 if (et > 0.5) {
563 st = 1;
564 et = et - 1;
565 }
566
567 double Bti[3];
568 double dBti[3];
569
570 double *ptBti = &Bti[0];
571 double *ptdBti = &dBti[0];
572 for (char it = -1; it <= 1; it++) {
573 *ptBti++ = Bspline3(-it + et);
574 *ptdBti++ = dBspline3(-it + et);
575 }
576
577 int NbParam_ = static_cast<int>(NbParam);
578 int ct_st_ = ct + st;
579 int cr_sr_ = cr + sr;
580
581 for (char ir = -1; ir <= 1; ir++) {
582 double Br = Bspline3(-ir + er);
583
584 int irInd = ir + 1;
585 int ind = (cr_sr_ + irInd) * Ncb;
586 ptBti = &Bti[0];
587 ptdBti = &dBti[0];
588
589 double Br_ptBti_ = Br * (*ptBti);
590 double Br_ptdBti_ = Br * (*ptdBti);
591 for (char it = -1; it <= 1; it++) {
592 Prt[ind + (ct_st_ + it)] += Br_ptBti_;
593 int ind1 = (ind + (ct_st_ + it)) * NbParam_;
594 for (short int ip = 0; ip < NbParam_; ip++) {
595 dPrt[ind1 + ip] -= Br_ptdBti_ * val[ip];
596 }
597 ptBti++;
598 ptdBti++;
599 }
600 }
601}
602
603void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,
604 int &Nc, double *val, unsigned int &NbParam)
605{
606 double Bti[4] = {0, 0, 0, 0};
607 double dBti[4] = {0, 0, 0, 0};
608
609 for (char it = -1; it <= 2; it++) {
610 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
611 dBti[it + 1] = dBspline4(-it + et);
612 }
613
614 int NbParam_ = (int)NbParam;
615
616 double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)];
617 for (int ir = -1; ir <= 2; ir++) {
618 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
619 for (int it = 0; it <= 3; it++) {
620 (*pt++) += Br * Bti[it];
621
622 double Br_dBti_ = Br * dBti[it];
623 for (int ip = 0; ip < NbParam_; ip++) {
624 (*pt++) -= Br_dBti_ * val[ip];
625 pt += NbParam_; // Modif AY
626 }
627 // pt=pt+NbParam*NbParam; // Modif AY
628 }
629 }
630}
631
632void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
633 double &et, int &Ncb, double *val, unsigned int &NbParam)
634{
635 double Bti[4] = {0, 0, 0, 0};
636 double dBti[4] = {0, 0, 0, 0};
637
638 for (char it = -1; it <= 2; it++) {
639 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
640 dBti[it + 1] = dBspline4(-it + et);
641 }
642
643 int NbParam_ = static_cast<int>(NbParam);
644
645 for (int ir = -1; ir <= 2; ir++) {
646 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
647 int irInd = ir + 1;
648 int ind = (cr + irInd) * Ncb;
649 int ind1 = ind + ct;
650
651 for (int it = 0; it <= 3; it++) {
652 Prt[ind1 + it] += Br * Bti[it];
653 int ind2 = (ind + (ct + it)) * NbParam_;
654
655 double Br_dBti_ = Br * dBti[it];
656 for (int ip = 0; ip < NbParam_; ip++) {
657 dPrt[ind2 + ip] -= Br_dBti_ * val[ip];
658 }
659 }
660 }
661}
662
663void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
664 int &Nc, unsigned int &NbParam, int &degree)
665{
666 switch (degree) {
667 case 4:
668 PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
669 break;
670 default:
671 PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
672 }
673}
674
675void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
676 int &Nc, unsigned int &NbParam)
677{
678 int sr = 0;
679 int st = 0;
680 if (er > 0.5) {
681 sr = 1;
682 er = er - 1;
683 }
684 if (et > 0.5) {
685 st = 1;
686 et = et - 1;
687 }
688
689 double Bti[3] = {0, 0, 0};
690
691 for (char it = -1; it <= 1; it++) {
692 Bti[it + 1] = Bspline3(-it + et);
693 }
694
695 int NbParam_ = static_cast<int>(NbParam);
696 int NbParam_val = NbParam_ + NbParam_ * NbParam_;
697
698 double *pt = &PrtTout[(unsigned int)(((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_val))];
699 for (int ir = -1; ir <= 1; ir++) {
700 double Br = Bspline3(-ir + er);
701 for (int it = 0; it <= 2; it++) {
702 (*pt++) += Br * Bti[it];
703 pt += NbParam_val;
704 }
705 }
706}
707void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
708 int &Nc, unsigned int &NbParam)
709{
710 double Bti[4] = {0, 0, 0, 0};
711
712 for (char it = -1; it <= 2; it++) {
713 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
714 }
715
716 int NbParam_ = static_cast<int>(NbParam);
717 int NbParam_val = NbParam_ + NbParam_ * NbParam_;
718 double *pt = &PrtTout[(unsigned int)((cr * Nc + ct) * 16 * (1 + NbParam_val))];
719 for (int ir = -1; ir <= 2; ir++) {
720 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
721 for (int it = 0; it <= 3; it++) {
722 (*pt++) += Br * Bti[it];
723 pt += NbParam_val;
724 }
725 }
726}
727
728void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb,
729 unsigned int &NbParam, int &degree)
730{
731 switch (degree) {
732 case 4:
733 PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
734 break;
735 default:
736 PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
737 }
738}
739
740void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
741{
742
743 int sr = 0;
744 int st = 0;
745 if (er > 0.5) {
746 sr = 1;
747 er = er - 1;
748 }
749 if (et > 0.5) {
750 st = 1;
751 et = et - 1;
752 }
753
754 double Bti[3] = {0, 0, 0};
755
756 for (char it = -1; it <= 1; it++) {
757 Bti[it + 1] = Bspline3(-it + et);
758 }
759
760 int ct_st_ = ct + st;
761 int cr_sr_ = cr + sr;
762 for (int ir = -1; ir <= 1; ir++) {
763 double Br = Bspline3(-ir + er);
764
765 int irInd = ir + 1;
766 int ind = (cr_sr_ + irInd) * Ncb;
767 for (int it = 0; it <= 2; it++) {
768 Prt[ind + (ct_st_ + it)] += Br * Bti[it];
769 }
770 }
771}
772
773void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
774{
775 double Bti[4] = {0, 0, 0, 0};
776
777 for (char it = -1; it <= 2; it++) {
778 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
779 }
780
781 for (int ir = -1; ir <= 2; ir++) {
782 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
783 int irInd = ir + 1;
784 int ind = (cr + irInd) * Ncb + ct;
785
786 for (int it = 0; it <= 3; it++) {
787 Prt[ind + it] += Br * Bti[it];
788 }
789 }
790}
791
792void vpTemplateTrackerMIBSpline::computeProbabilities(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
793 double *dW, unsigned int &NbParam, int &bspline,
795 bool use_hessien_des)
796{
797 if (approx == vpTemplateTrackerMI::HESSIAN_NONSECOND || use_hessien_des) {
798 if (bspline == 3)
799 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
800 else
801 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
802 } else {
803 if (bspline == 3)
804 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, dW, NbParam);
805 else
806 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, dW, NbParam);
807 }
808}
809
810#endif