39#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
41#ifndef DOXYGEN_SHOULD_SKIP_THIS
43void vpTemplateTrackerMIBSpline::PutPVBsplineD(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val,
48 PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
51 PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);
55void vpTemplateTrackerMIBSpline::PutPVBsplineD3(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val)
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;
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);
85void vpTemplateTrackerMIBSpline::PutPVBsplineD4(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val)
89 double *ptBti = &Bti[0];
90 for (
int it = -1; it <= 2; it++) {
91 *ptBti++ = Bspline4i(it - et, it);
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);
99 for (
int it = -1; it <= 2; it++) {
100 *pt++ += Br * (*ptBti++) * val;
104 for (
int ir = -1; ir <= 2; ir++) {
105 double Br = Bspline4i(ir - er, ir);
107 for (
int it = -1; it <= 2; it++) {
108 *pt++ += Br * (*ptBti++);
114double vpTemplateTrackerMIBSpline::Bspline3(
double diff)
116 double aDiff = std::fabs(diff);
120 return (-(aDiff * aDiff) + 0.75);
122 double tmp_ = 1.5 - aDiff;
123 return (0.5 * tmp_ * tmp_);
129double vpTemplateTrackerMIBSpline::Bspline4i(
double diff,
int &interv)
134 double tmp_ = 2. + diff;
135 return (tmp_ * tmp_ * tmp_ / 6.);
138 double diff2_ = diff * diff;
139 return (-diff2_ * diff / 2. - diff2_ + 4. / 6.);
142 double diff2_ = diff * diff;
143 return (diff2_ * diff / 2. - diff2_ + 4. / 6.);
150double vpTemplateTrackerMIBSpline::dBspline3(
double diff)
152 if ((diff > -1.5) && (diff <= -0.5))
154 else if ((diff > -0.5) && (diff <= 0.5))
156 else if ((diff > 0.5) && (diff <= 1.5))
162double vpTemplateTrackerMIBSpline::dBspline4(
double diff)
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_);
179double vpTemplateTrackerMIBSpline::d2Bspline3(
double diff)
181 if ((diff > -1.5) && (diff <= -0.5))
183 else if ((diff > -0.5) && (diff <= 0.5))
185 else if ((diff > 0.5) && (diff <= 1.5))
191double vpTemplateTrackerMIBSpline::d2Bspline4(
double diff)
193 if ((diff > -2.) && (diff <= -1.))
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.))
205void vpTemplateTrackerMIBSpline::PutTotPVBspline(
double *Prt,
int cr,
double &er,
int ct,
double &et,
int Nc,
206 double *val,
unsigned int &NbParam,
int °ree)
210 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
213 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
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 °ree)
222 PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
225 PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
229void vpTemplateTrackerMIBSpline::PutTotPVBspline3(
double *Prt,
int cr,
double &er,
int ct,
double &et,
int Nc,
230 double *val,
unsigned int &NbParam)
247 double *ptBti = &Bti[0];
248 double *ptdBti = &dBti[0];
249 double *ptd2Bti = &d2Bti[0];
251 for (
short int it = 1; it >= -1; it--) {
252 *ptBti++ = Bspline3(it + et);
253 *ptdBti++ = dBspline3(it + et);
254 *ptd2Bti++ = d2Bspline3(it + et);
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);
261 for (
short unsigned int it = 0; it <= 2; ++it) {
262 *pt++ += Br * (Bti[it]);
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];
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)
293 double *ptBti = &Bti[0];
294 double *ptdBti = &dBti[0];
295 double *ptd2Bti = &d2Bti[0];
297 for (
short int it = 1; it >= -1; it--) {
298 *ptBti++ = Bspline3(it + et);
299 *ptdBti++ = dBspline3(it + et);
300 *ptd2Bti++ = d2Bspline3(it + et);
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]);
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];
324void vpTemplateTrackerMIBSpline::PutTotPVBspline3(
double *Prt,
double &er,
double *bt,
unsigned int size)
328 double *bt0 = &bt[0];
333 for (
int ir = -1; ir <= 1; ++ir) {
334 double Br = Bspline3(-ir + er);
335 const double *btend = bt0 + size;
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];
356 for (; bt < btend; *Prt++ += Br * *bt++) {
362void vpTemplateTrackerMIBSpline::PutTotPVBspline4(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
363 double *val,
unsigned int &NbParam)
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);
378 int NbParam_ = (int)NbParam;
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);
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);
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];
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)
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);
420 int NbParam_ = (int)NbParam;
422 for (
int ir = -1; ir <= 2; ir++) {
423 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
425 int ind = (cr + irInd) * Ncb + ct;
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];
449void vpTemplateTrackerMIBSpline::PutTotPVBspline4(
double *Prt,
double &er,
double *bt,
unsigned int size)
452 double *bt0 = &bt[0];
454 for (
int ir = -1; ir <= 2; ++ir) {
455 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
456 const double *btend = bt0 + size;
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];
477 for (; bt < btend; *Prt++ += Br * *bt++) {
483void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
484 double *val,
unsigned int &NbParam,
int °ree)
488 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
491 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
495void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
496 double &et,
int &Ncb,
double *val,
unsigned int &NbParam,
501 PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
504 PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
508void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
509 int &Nc,
double *val,
unsigned int &NbParam)
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);
532 int NbParam_ = (int)NbParam;
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);
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];
553void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
554 double &et,
int &Ncb,
double *val,
unsigned int &NbParam)
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);
577 int NbParam_ =
static_cast<int>(NbParam);
578 int ct_st_ = ct + st;
579 int cr_sr_ = cr + sr;
581 for (
char ir = -1; ir <= 1; ir++) {
582 double Br = Bspline3(-ir + er);
585 int ind = (cr_sr_ + irInd) * Ncb;
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];
603void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
604 int &Nc,
double *val,
unsigned int &NbParam)
606 double Bti[4] = {0, 0, 0, 0};
607 double dBti[4] = {0, 0, 0, 0};
609 for (
char it = -1; it <= 2; it++) {
610 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
611 dBti[it + 1] = dBspline4(-it + et);
614 int NbParam_ = (int)NbParam;
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];
622 double Br_dBti_ = Br * dBti[it];
623 for (
int ip = 0; ip < NbParam_; ip++) {
624 (*pt++) -= Br_dBti_ * val[ip];
632void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
633 double &et,
int &Ncb,
double *val,
unsigned int &NbParam)
635 double Bti[4] = {0, 0, 0, 0};
636 double dBti[4] = {0, 0, 0, 0};
638 for (
char it = -1; it <= 2; it++) {
639 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
640 dBti[it + 1] = dBspline4(-it + et);
643 int NbParam_ =
static_cast<int>(NbParam);
645 for (
int ir = -1; ir <= 2; ir++) {
646 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
648 int ind = (cr + irInd) * Ncb;
651 for (
int it = 0; it <= 3; it++) {
652 Prt[ind1 + it] += Br * Bti[it];
653 int ind2 = (ind + (ct + it)) * NbParam_;
655 double Br_dBti_ = Br * dBti[it];
656 for (
int ip = 0; ip < NbParam_; ip++) {
657 dPrt[ind2 + ip] -= Br_dBti_ * val[ip];
663void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
664 int &Nc,
unsigned int &NbParam,
int °ree)
668 PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
671 PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
675void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
676 int &Nc,
unsigned int &NbParam)
689 double Bti[3] = {0, 0, 0};
691 for (
char it = -1; it <= 1; it++) {
692 Bti[it + 1] = Bspline3(-it + et);
695 int NbParam_ =
static_cast<int>(NbParam);
696 int NbParam_val = NbParam_ + NbParam_ * NbParam_;
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];
707void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
708 int &Nc,
unsigned int &NbParam)
710 double Bti[4] = {0, 0, 0, 0};
712 for (
char it = -1; it <= 2; it++) {
713 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
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];
728void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb,
729 unsigned int &NbParam,
int °ree)
733 PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
736 PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
740void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb)
754 double Bti[3] = {0, 0, 0};
756 for (
char it = -1; it <= 1; it++) {
757 Bti[it + 1] = Bspline3(-it + et);
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);
766 int ind = (cr_sr_ + irInd) * Ncb;
767 for (
int it = 0; it <= 2; it++) {
768 Prt[ind + (ct_st_ + it)] += Br * Bti[it];
773void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb)
775 double Bti[4] = {0, 0, 0, 0};
777 for (
char it = -1; it <= 2; it++) {
778 Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
781 for (
int ir = -1; ir <= 2; ir++) {
782 double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
784 int ind = (cr + irInd) * Ncb + ct;
786 for (
int it = 0; it <= 3; it++) {
787 Prt[ind + it] += Br * Bti[it];
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)
799 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
801 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
804 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, dW, NbParam);
806 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, dW, NbParam);
vpHessienApproximationType