Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
vpTemplateTrackerMIBSpline.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Template tracker.
32 */
33
34#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
35
36#ifndef DOXYGEN_SHOULD_SKIP_THIS
37BEGIN_VISP_NAMESPACE
38void vpTemplateTrackerMIBSpline::PutPVBsplineD(double *Prt, int cr, double er, int ct, double et, int Nc, double val,
39 const int &degre)
40{
41 switch (degre) {
42 case 4:
43 PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
44 break;
45 default:
46 PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);
47 }
48}
49
50void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
51{
52 int sr = 0;
53 int st = 0;
54 if (er > 0.5) {
55 sr = 1;
56 er = er - 1;
57 }
58 if (et > 0.5) {
59 st = 1;
60 et = et - 1;
61 }
62 double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9];
63 if (std::fabs(val - 1.) > std::numeric_limits<double>::epsilon()) {
64 for (int ir = -1; ir <= 1; ir++) {
65 double Bspline3_diff_r_ = Bspline3(ir - er);
66 for (int it = -1; it <= 1; it++) {
67 *pt++ += Bspline3_diff_r_ * Bspline3(it - et) * val;
68 }
69 }
70 }
71 else {
72 for (int ir = -1; ir <= 1; ir++) {
73 double Bspline3_diff_r_ = Bspline3(ir - er);
74 for (int it = -1; it <= 1; it++) {
75 *pt++ += Bspline3_diff_r_ * Bspline3(it - et);
76 }
77 }
78 }
79}
80
81void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
82{
83 double Bti[4];
84
85 double *ptBti = &Bti[0];
86 for (int it = -1; it <= 2; it++) {
87 *ptBti++ = Bspline4i(it - et, it);
88 // pt++;
89 }
90 double *pt = &Prt[(cr * Nc + ct) * 16];
91 if (std::fabs(val - 1.) > std::numeric_limits<double>::epsilon()) {
92 for (int ir = -1; ir <= 2; ir++) {
93 double Br = Bspline4i(ir - er, ir);
94 ptBti = &Bti[0];
95 for (int it = -1; it <= 2; it++) {
96 *pt++ += Br * (*ptBti++) * val;
97 }
98 }
99 }
100 else {
101 for (int ir = -1; ir <= 2; ir++) {
102 double Br = Bspline4i(ir - er, ir);
103 ptBti = &Bti[0];
104 for (int it = -1; it <= 2; it++) {
105 *pt++ += Br * (*ptBti++);
106 }
107 }
108 }
109}
110
111double vpTemplateTrackerMIBSpline::Bspline3(double diff)
112{
113 double aDiff = std::fabs(diff);
114
115 if (aDiff < 1.5) {
116 if (aDiff < 0.5) {
117 return (-(aDiff * aDiff) + 0.75);
118 }
119 double tmp_ = 1.5 - aDiff;
120 return (0.5 * tmp_ * tmp_);
121 }
122
123 return 0;
124}
125
126double vpTemplateTrackerMIBSpline::Bspline4i(double diff, int &interv)
127{
128 switch (interv) {
129 case 2:
130 case -1: {
131 double tmp_ = 2. + diff;
132 return (tmp_ * tmp_ * tmp_ / 6.);
133 }
134 case 0: {
135 double diff2_ = diff * diff;
136 return (-diff2_ * diff / 2. - diff2_ + 4. / 6.);
137 }
138 case 1: {
139 double diff2_ = diff * diff;
140 return (diff2_ * diff / 2. - diff2_ + 4. / 6.);
141 }
142 default:
143 return 0;
144 }
145}
146
147double vpTemplateTrackerMIBSpline::dBspline3(double diff)
148{
149 if ((diff > -1.5) && (diff <= -0.5))
150 return diff + 1.5;
151 else if ((diff > -0.5) && (diff <= 0.5))
152 return -2. * diff;
153 else if ((diff > 0.5) && (diff <= 1.5))
154 return diff - 1.5;
155
156 return 0;
157}
158
159double vpTemplateTrackerMIBSpline::dBspline4(double diff)
160{
161 if ((diff > -2.) && (diff <= -1.)) {
162 double diff_2_ = diff + 2.;
163 return (diff_2_ * diff_2_ * 0.5);
164 }
165 else if ((diff > -1.) && (diff <= 0.)) {
166 return -1.5 * diff * diff - 2. * diff;
167 }
168 else if ((diff > 0.) && (diff <= 1.)) {
169 return 1.5 * diff * diff - 2. * diff;
170 }
171 else if ((diff > 1.) && (diff <= 2.)) {
172 double diff_2_ = diff - 2.;
173 return (-0.5 * diff_2_ * diff_2_);
174 }
175 else {
176 return 0;
177 }
178}
179
180double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
181{
182 if ((diff > -1.5) && (diff <= -0.5))
183 return 1.;
184 else if ((diff > -0.5) && (diff <= 0.5))
185 return -2.;
186 else if ((diff > 0.5) && (diff <= 1.5))
187 return 1.;
188 else
189 return 0;
190}
191
192double vpTemplateTrackerMIBSpline::d2Bspline4(double diff)
193{
194 if ((diff > -2.) && (diff <= -1.))
195 return (diff + 2.);
196 else if ((diff > -1.) && (diff <= 0.))
197 return -3. * diff - 2.;
198 else if ((diff > 0.) && (diff <= 1.))
199 return 3. * diff - 2.;
200 else if ((diff > 1.) && (diff <= 2.))
201 return -(diff - 2.);
202 else
203 return 0;
204}
205
206void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et, int Nc,
207 double *val, unsigned int &NbParam, int &degree)
208{
209 switch (degree) {
210 case 4:
211 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
212 break;
213 default:
214 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
215 }
216}
217
218void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct,
219 double &et, int Ncb, double *val, unsigned int &NbParam, int &degree)
220{
221 switch (degree) {
222 case 4:
223 PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
224 break;
225 default:
226 PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
227 }
228}
229
230void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et, int Nc,
231 double *val, unsigned int &NbParam)
232{
233 short int sr = 0;
234 short int st = 0;
235 if (er > 0.5) {
236 sr = 1;
237 er = er - 1;
238 }
239 if (et > 0.5) {
240 st = 1;
241 et = et - 1;
242 }
243
244 double Bti[3];
245 double dBti[3];
246 double d2Bti[3];
247
248 double *ptBti = &Bti[0];
249 double *ptdBti = &dBti[0];
250 double *ptd2Bti = &d2Bti[0];
251
252 for (short int it = 1; it >= -1; it--) {
253 *ptBti++ = Bspline3(it + et);
254 *ptdBti++ = dBspline3(it + et);
255 *ptd2Bti++ = d2Bspline3(it + et);
256 }
257
258 double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + static_cast<int>(NbParam + NbParam * NbParam))];
259 for (short int ir = -1; ir <= 1; ++ir) {
260 double Br = Bspline3(-ir + er);
261
262 for (short unsigned int it = 0; it <= 2; ++it) {
263 *pt++ += Br * (Bti[it]);
264
265 double v1 = Br * (dBti[it]);
266 for (short unsigned int ip = 0; ip < NbParam; ++ip) {
267 *pt++ -= v1 * val[ip];
268 double v2 = Br * (d2Bti[it]) * val[ip];
269 for (short unsigned int ip2 = 0; ip2 < NbParam; ++ip2)
270 *pt++ += v2 * val[ip2];
271 }
272 }
273 }
274}
275
276void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct,
277 double &et, int Ncb, double *val, unsigned int &NbParam)
278{
279 short int sr = 0;
280 short int st = 0;
281 if (er > 0.5) {
282 sr = 1;
283 er = er - 1;
284 }
285 if (et > 0.5) {
286 st = 1;
287 et = et - 1;
288 }
289
290 double Bti[3];
291 double dBti[3];
292 double d2Bti[3];
293
294 double *ptBti = &Bti[0];
295 double *ptdBti = &dBti[0];
296 double *ptd2Bti = &d2Bti[0];
297
298 for (short int it = 1; it >= -1; it--) {
299 *ptBti++ = Bspline3(it + et);
300 *ptdBti++ = dBspline3(it + et);
301 *ptd2Bti++ = d2Bspline3(it + et);
302 }
303
304 int NbParam_ = static_cast<int>(NbParam);
305 for (short int ir = -1; ir <= 1; ++ir) {
306 double Br = Bspline3(-ir + er);
307 short int irInd = ir + 1;
308 short int ind = (cr + sr + irInd) * Ncb;
309 for (short int it = 0; it <= 2; ++it) {
310 Prt[ind + (ct + st + it)] += Br * (Bti[it]);
311
312 double v1 = Br * (dBti[it]);
313 int ind1 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_;
314 for (int ip = 0; ip < NbParam_; ++ip) {
315 dPrt[ind1 + ip] -= v1 * val[ip];
316 double v2 = Br * (d2Bti[it]) * val[ip];
317 int ind2 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_ * NbParam_ + ip * NbParam_;
318 for (short int ip2 = 0; ip2 < NbParam_; ++ip2)
319 d2Prt[ind2 + ip2] += v2 * val[ip2];
320 }
321 }
322 }
323}
324
325void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size)
326{
327#define LSIZE 12
328
329 double *bt0 = &bt[0];
330 if (er > 0.5) {
331 er = er - 1.0;
332 }
333
334 for (int ir = -1; ir <= 1; ++ir) {
335 double Br = Bspline3(-ir + er);
336 const double *btend = bt0 + size;
337 bt = bt0;
338
339 if (size >= LSIZE) {
340 btend -= LSIZE - 1;
341 for (; bt < btend; bt += LSIZE) {
342 *Prt++ += Br * bt[0];
343 *Prt++ += Br * bt[1];
344 *Prt++ += Br * bt[2];
345 *Prt++ += Br * bt[3];
346 *Prt++ += Br * bt[4];
347 *Prt++ += Br * bt[5];
348 *Prt++ += Br * bt[6];
349 *Prt++ += Br * bt[7];
350 *Prt++ += Br * bt[8];
351 *Prt++ += Br * bt[9];
352 *Prt++ += Br * bt[10];
353 *Prt++ += Br * bt[11];
354 }
355 btend += LSIZE - 1;
356 }
357 for (; bt < btend; *Prt++ += Br * *bt++) {
358 }
359 }
360#undef LSIZE
361}
362
363void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et, int Nc,
364 double *val, unsigned int &NbParam)
365{
366 double Bti[4];
367 double dBti[4];
368 double d2Bti[4];
369
370 double *ptBti = &Bti[0];
371 double *ptdBti = &dBti[0];
372 double *ptd2Bti = &d2Bti[0];
373 for (char it = -1; it <= 2; it++) {
374 *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
375 *ptdBti++ = dBspline4(-it + et);
376 *ptd2Bti++ = d2Bspline4(-it + et);
377 }
378
379 int NbParam_ = static_cast<int>(NbParam);
380
381 double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)];
382 for (char ir = -1; ir <= 2; ir++) {
383 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
384 ptBti = &Bti[0];
385 ptdBti = &dBti[0];
386 ptd2Bti = &d2Bti[0];
387 for (char it = -1; it <= 2; it++) {
388 double Br_ptBti_ = Br * (*ptBti);
389 double Br_ptdBti_ = Br * (*ptdBti);
390 double Br_ptd2Bti_ = Br * (*ptd2Bti);
391 *pt++ += Br_ptBti_;
392 for (short int ip = 0; ip < NbParam_; ip++) {
393 *pt++ -= Br_ptdBti_ * val[ip];
394 for (short int ip2 = 0; ip2 < NbParam_; ip2++) {
395 *pt++ += Br_ptd2Bti_ * val[ip] * val[ip2];
396 }
397 }
398 ptBti++;
399 ptdBti++;
400 ptd2Bti++;
401 }
402 }
403}
404
405void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct,
406 double et, int Ncb, double *val, unsigned int &NbParam)
407{
408 double Bti[4];
409 double dBti[4];
410 double d2Bti[4];
411
412 for (size_t i = 0; i < 4; ++i) {
413 Bti[i] = 0.;
414 dBti[i] = 0.;
415 d2Bti[i] = 0.;
416 }
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_ = static_cast<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
456void 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
490void 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
502void 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
515void 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_ = static_cast<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
560void 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
610void 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_ = static_cast<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
639void 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
670void 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
682void 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[static_cast<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}
714void 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[static_cast<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
735void 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
747void 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
780void 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
799void vpTemplateTrackerMIBSpline::computeProbabilities(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
800 double *dW, unsigned int &NbParam, int &bspline,
801 vpTemplateTrackerMI::vpHessienApproximationType &approx,
802 bool use_hessien_des)
803{
804 if (approx == vpTemplateTrackerMI::HESSIAN_NONSECOND || use_hessien_des) {
805 if (bspline == 3)
806 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
807 else
808 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
809 }
810 else {
811 if (bspline == 3)
812 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, dW, NbParam);
813 else
814 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, dW, NbParam);
815 }
816}
817END_VISP_NAMESPACE
818#endif
BEGIN_VISP_NAMESPACE
Definition vpMbtDistanceCircle.cpp:55