40 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h> 42 #ifndef DOXYGEN_SHOULD_SKIP_THIS 44 void vpTemplateTrackerMIBSpline::PutPVBsplineD(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val,
49 PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
52 PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);
56 void vpTemplateTrackerMIBSpline::PutPVBsplineD3(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val)
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;
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);
87 void vpTemplateTrackerMIBSpline::PutPVBsplineD4(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val)
91 double *ptBti = &Bti[0];
92 for (
int it = -1; it <= 2; it++) {
93 *ptBti++ = Bspline4i(it - et, it);
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);
101 for (
int it = -1; it <= 2; it++) {
102 *pt++ += Br * (*ptBti++) * val;
107 for (
int ir = -1; ir <= 2; ir++) {
108 double Br = Bspline4i(ir - er, ir);
110 for (
int it = -1; it <= 2; it++) {
111 *pt++ += Br * (*ptBti++);
117 double vpTemplateTrackerMIBSpline::Bspline3(
double diff)
119 double aDiff = std::fabs(diff);
123 return (-(aDiff * aDiff) + 0.75);
125 double tmp_ = 1.5 - aDiff;
126 return (0.5 * tmp_ * tmp_);
132 double vpTemplateTrackerMIBSpline::Bspline4i(
double diff,
int &interv)
137 double tmp_ = 2. + diff;
138 return (tmp_ * tmp_ * tmp_ / 6.);
141 double diff2_ = diff * diff;
142 return (-diff2_ * diff / 2. - diff2_ + 4. / 6.);
145 double diff2_ = diff * diff;
146 return (diff2_ * diff / 2. - diff2_ + 4. / 6.);
153 double vpTemplateTrackerMIBSpline::dBspline3(
double diff)
155 if ((diff > -1.5) && (diff <= -0.5))
157 else if ((diff > -0.5) && (diff <= 0.5))
159 else if ((diff > 0.5) && (diff <= 1.5))
165 double vpTemplateTrackerMIBSpline::dBspline4(
double diff)
167 if ((diff > -2.) && (diff <= -1.)) {
168 double diff_2_ = diff + 2.;
169 return (diff_2_ * diff_2_ * 0.5);
171 else if ((diff > -1.) && (diff <= 0.)) {
172 return -1.5 * diff * diff - 2. * diff;
174 else if ((diff > 0.) && (diff <= 1.)) {
175 return 1.5 * diff * diff - 2. * diff;
177 else if ((diff > 1.) && (diff <= 2.)) {
178 double diff_2_ = diff - 2.;
179 return (- 0.5 * diff_2_ * diff_2_);
186 double vpTemplateTrackerMIBSpline::d2Bspline3(
double diff)
188 if ((diff > -1.5) && (diff <= -0.5))
190 else if ((diff > -0.5) && (diff <= 0.5))
192 else if ((diff > 0.5) && (diff <= 1.5))
198 double vpTemplateTrackerMIBSpline::d2Bspline4(
double diff)
200 if ((diff > -2.) && (diff <= -1.))
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.))
212 void vpTemplateTrackerMIBSpline::PutTotPVBspline(
double *Prt,
int cr,
double &er,
int ct,
double &et,
int Nc,
213 double *val,
unsigned int &NbParam,
int °ree)
217 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
220 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
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 °ree)
229 PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
232 PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
236 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(
double *Prt,
int cr,
double &er,
int ct,
double &et,
int Nc,
237 double *val,
unsigned int &NbParam)
254 double *ptBti = &Bti[0];
255 double *ptdBti = &dBti[0];
256 double *ptd2Bti = &d2Bti[0];
258 for (
short int it = 1; it >= -1; it--) {
259 *ptBti++ = Bspline3(it + et);
260 *ptdBti++ = dBspline3(it + et);
261 *ptd2Bti++ = d2Bspline3(it + et);
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);
268 for (
short unsigned int it = 0; it <= 2; ++it) {
269 *pt++ += Br * (Bti[it]);
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];
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)
300 double *ptBti = &Bti[0];
301 double *ptdBti = &dBti[0];
302 double *ptd2Bti = &d2Bti[0];
304 for (
short int it = 1; it >= -1; it--) {
305 *ptBti++ = Bspline3(it + et);
306 *ptdBti++ = dBspline3(it + et);
307 *ptd2Bti++ = d2Bspline3(it + et);
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]);
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];
331 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(
double *Prt,
double &er,
double *bt,
unsigned int size)
335 double *bt0 = &bt[0];
340 for (
int ir = -1; ir <= 1; ++ir) {
341 double Br = Bspline3(-ir + er);
342 const double *btend = bt0 + size;
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];
363 for (; bt < btend; *Prt++ += Br * *bt++) {
369 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
370 double *val,
unsigned int &NbParam)
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);
385 int NbParam_ = (int)NbParam;
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);
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);
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];
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)
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);
427 int NbParam_ = (int)NbParam;
429 for (
int ir = -1; ir <= 2; ir++) {
430 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
432 int ind = (cr + irInd) * Ncb + ct;
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];
456 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(
double *Prt,
double &er,
double *bt,
unsigned int size)
459 double *bt0 = &bt[0];
461 for (
int ir = -1; ir <= 2; ++ir) {
462 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
463 const double *btend = bt0 + size;
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];
484 for (; bt < btend; *Prt++ += Br * *bt++) {
490 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
491 double *val,
unsigned int &NbParam,
int °ree)
495 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
498 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
502 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
503 double &et,
int &Ncb,
double *val,
unsigned int &NbParam,
508 PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
511 PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
515 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
516 int &Nc,
double *val,
unsigned int &NbParam)
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);
539 int NbParam_ = (int)NbParam;
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);
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];
560 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
561 double &et,
int &Ncb,
double *val,
unsigned int &NbParam)
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);
584 int NbParam_ =
static_cast<int>(NbParam);
585 int ct_st_ = ct + st;
586 int cr_sr_ = cr + sr;
588 for (
char ir = -1; ir <= 1; ir++) {
589 double Br = Bspline3(-ir + er);
592 int ind = (cr_sr_ + irInd) * Ncb;
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];
610 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
611 int &Nc,
double *val,
unsigned int &NbParam)
613 double Bti[4] = {0, 0, 0, 0};
614 double dBti[4] = {0, 0, 0, 0};
616 for (
char it = -1; it <= 2; it++) {
617 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
618 dBti[it + 1] = dBspline4(-it + et);
621 int NbParam_ = (int)NbParam;
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];
629 double Br_dBti_ = Br * dBti[it];
630 for (
int ip = 0; ip < NbParam_; ip++) {
631 (*pt++) -= Br_dBti_ * val[ip];
639 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
640 double &et,
int &Ncb,
double *val,
unsigned int &NbParam)
642 double Bti[4] = {0, 0, 0, 0};
643 double dBti[4] = {0, 0, 0, 0};
645 for (
char it = -1; it <= 2; it++) {
646 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
647 dBti[it + 1] = dBspline4(-it + et);
650 int NbParam_ =
static_cast<int>(NbParam);
652 for (
int ir = -1; ir <= 2; ir++) {
653 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
655 int ind = (cr + irInd) * Ncb;
658 for (
int it = 0; it <= 3; it++) {
659 Prt[ind1 + it] += Br * Bti[it];
660 int ind2 = (ind + (ct + it)) * NbParam_;
662 double Br_dBti_ = Br * dBti[it];
663 for (
int ip = 0; ip < NbParam_; ip++) {
664 dPrt[ind2 + ip] -= Br_dBti_ * val[ip];
670 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
671 int &Nc,
unsigned int &NbParam,
int °ree)
675 PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
678 PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
682 void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
683 int &Nc,
unsigned int &NbParam)
696 double Bti[3] = {0, 0, 0};
698 for (
char it = -1; it <= 1; it++) {
699 Bti[it + 1] = Bspline3(-it + et);
702 int NbParam_ =
static_cast<int>(NbParam);
703 int NbParam_val = NbParam_ + NbParam_ * NbParam_;
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];
714 void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
715 int &Nc,
unsigned int &NbParam)
717 double Bti[4] = {0, 0, 0, 0};
719 for (
char it = -1; it <= 2; it++) {
720 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
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];
735 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb,
736 unsigned int &NbParam,
int °ree)
740 PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
743 PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
747 void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb)
761 double Bti[3] = {0, 0, 0};
763 for (
char it = -1; it <= 1; it++) {
764 Bti[it + 1] = Bspline3(-it + et);
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);
773 int ind = (cr_sr_ + irInd) * Ncb;
774 for (
int it = 0; it <= 2; it++) {
775 Prt[ind + (ct_st_ + it)] += Br * Bti[it];
780 void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb)
782 double Bti[4] = {0, 0, 0, 0};
784 for (
char it = -1; it <= 2; it++) {
785 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
788 for (
int ir = -1; ir <= 2; ir++) {
789 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
791 int ind = (cr + irInd) * Ncb + ct;
793 for (
int it = 0; it <= 3; it++) {
794 Prt[ind + it] += Br * Bti[it];
799 void vpTemplateTrackerMIBSpline::computeProbabilities(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
double *dW,
804 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
806 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
810 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, dW, NbParam);
812 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, dW, NbParam);
vpHessienApproximationType