1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Fabien Spindler
38 *
39 *****************************************************************************/
40 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
41
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
43
PutPVBsplineD(double * Prt,int cr,double er,int ct,double et,int Nc,double val,const int & degre)44 void vpTemplateTrackerMIBSpline::PutPVBsplineD(double *Prt, int cr, double er, int ct, double et, int Nc, double val,
45 const int °re)
46 {
47 switch (degre) {
48 case 4:
49 PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
50 break;
51 default:
52 PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);
53 }
54 }
55
PutPVBsplineD3(double * Prt,int cr,double er,int ct,double et,int Nc,double val)56 void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
57 {
58 int sr = 0;
59 int st = 0;
60 if (er > 0.5) {
61 sr = 1;
62 er = er - 1;
63 }
64 if (et > 0.5) {
65 st = 1;
66 et = et - 1;
67 }
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;
74 }
75 }
76 }
77 else {
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);
82 }
83 }
84 }
85 }
86
PutPVBsplineD4(double * Prt,int cr,double er,int ct,double et,int Nc,double val)87 void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
88 {
89 double Bti[4];
90
91 double *ptBti = &Bti[0];
92 for (int it = -1; it <= 2; it++) {
93 *ptBti++ = Bspline4i(it - et, it);
94 // pt++;
95 }
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);
100 ptBti = &Bti[0];
101 for (int it = -1; it <= 2; it++) {
102 *pt++ += Br * (*ptBti++) * val;
103 }
104 }
105 }
106 else {
107 for (int ir = -1; ir <= 2; ir++) {
108 double Br = Bspline4i(ir - er, ir);
109 ptBti = &Bti[0];
110 for (int it = -1; it <= 2; it++) {
111 *pt++ += Br * (*ptBti++);
112 }
113 }
114 }
115 }
116
Bspline3(double diff)117 double vpTemplateTrackerMIBSpline::Bspline3(double diff)
118 {
119 double aDiff = std::fabs(diff);
120
121 if (aDiff < 1.5) {
122 if (aDiff < 0.5) {
123 return (-(aDiff * aDiff) + 0.75);
124 }
125 double tmp_ = 1.5 - aDiff;
126 return (0.5 * tmp_ * tmp_);
127 }
128
129 return 0;
130 }
131
Bspline4i(double diff,int & interv)132 double vpTemplateTrackerMIBSpline::Bspline4i(double diff, int &interv)
133 {
134 switch (interv) {
135 case 2:
136 case -1: {
137 double tmp_ = 2. + diff;
138 return (tmp_ * tmp_ * tmp_ / 6.);
139 }
140 case 0: {
141 double diff2_ = diff * diff;
142 return (-diff2_ * diff / 2. - diff2_ + 4. / 6.);
143 }
144 case 1: {
145 double diff2_ = diff * diff;
146 return (diff2_ * diff / 2. - diff2_ + 4. / 6.);
147 }
148 default:
149 return 0;
150 }
151 }
152
dBspline3(double diff)153 double vpTemplateTrackerMIBSpline::dBspline3(double diff)
154 {
155 if ((diff > -1.5) && (diff <= -0.5))
156 return diff + 1.5;
157 else if ((diff > -0.5) && (diff <= 0.5))
158 return -2. * diff;
159 else if ((diff > 0.5) && (diff <= 1.5))
160 return diff - 1.5;
161
162 return 0;
163 }
164
dBspline4(double diff)165 double vpTemplateTrackerMIBSpline::dBspline4(double diff)
166 {
167 if ((diff > -2.) && (diff <= -1.)) {
168 double diff_2_ = diff + 2.;
169 return (diff_2_ * diff_2_ * 0.5);
170 }
171 else if ((diff > -1.) && (diff <= 0.)) {
172 return -1.5 * diff * diff - 2. * diff;
173 }
174 else if ((diff > 0.) && (diff <= 1.)) {
175 return 1.5 * diff * diff - 2. * diff;
176 }
177 else if ((diff > 1.) && (diff <= 2.)) {
178 double diff_2_ = diff - 2.;
179 return (- 0.5 * diff_2_ * diff_2_);
180 }
181 else {
182 return 0;
183 }
184 }
185
d2Bspline3(double diff)186 double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
187 {
188 if ((diff > -1.5) && (diff <= -0.5))
189 return 1.;
190 else if ((diff > -0.5) && (diff <= 0.5))
191 return -2.;
192 else if ((diff > 0.5) && (diff <= 1.5))
193 return 1.;
194 else
195 return 0;
196 }
197
d2Bspline4(double diff)198 double vpTemplateTrackerMIBSpline::d2Bspline4(double diff)
199 {
200 if ((diff > -2.) && (diff <= -1.))
201 return (diff + 2.);
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.))
207 return -(diff - 2.);
208 else
209 return 0;
210 }
211
PutTotPVBspline(double * Prt,int cr,double & er,int ct,double & et,int Nc,double * val,unsigned int & NbParam,int & degree)212 void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et, int Nc,
213 double *val, unsigned int &NbParam, int °ree)
214 {
215 switch (degree) {
216 case 4:
217 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
218 break;
219 default:
220 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
221 }
222 }
223
PutTotPVBspline(double * Prt,double * dPrt,double * d2Prt,int cr,double & er,int ct,double & et,int Ncb,double * val,unsigned int & NbParam,int & degree)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)
226 {
227 switch (degree) {
228 case 4:
229 PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
230 break;
231 default:
232 PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
233 }
234 }
235
PutTotPVBspline3(double * Prt,int cr,double & er,int ct,double & et,int Nc,double * val,unsigned int & NbParam)236 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et, int Nc,
237 double *val, unsigned int &NbParam)
238 {
239 short int sr = 0;
240 short int st = 0;
241 if (er > 0.5) {
242 sr = 1;
243 er = er - 1;
244 }
245 if (et > 0.5) {
246 st = 1;
247 et = et - 1;
248 }
249
250 double Bti[3];
251 double dBti[3];
252 double d2Bti[3];
253
254 double *ptBti = &Bti[0];
255 double *ptdBti = &dBti[0];
256 double *ptd2Bti = &d2Bti[0];
257
258 for (short int it = 1; it >= -1; it--) {
259 *ptBti++ = Bspline3(it + et);
260 *ptdBti++ = dBspline3(it + et);
261 *ptd2Bti++ = d2Bspline3(it + et);
262 }
263
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);
267
268 for (short unsigned int it = 0; it <= 2; ++it) {
269 *pt++ += Br * (Bti[it]);
270
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];
277 }
278 }
279 }
280 }
281
PutTotPVBspline3(double * Prt,double * dPrt,double * d2Prt,int cr,double & er,int ct,double & et,int Ncb,double * val,unsigned int & NbParam)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)
284 {
285 short int sr = 0;
286 short int st = 0;
287 if (er > 0.5) {
288 sr = 1;
289 er = er - 1;
290 }
291 if (et > 0.5) {
292 st = 1;
293 et = et - 1;
294 }
295
296 double Bti[3];
297 double dBti[3];
298 double d2Bti[3];
299
300 double *ptBti = &Bti[0];
301 double *ptdBti = &dBti[0];
302 double *ptd2Bti = &d2Bti[0];
303
304 for (short int it = 1; it >= -1; it--) {
305 *ptBti++ = Bspline3(it + et);
306 *ptdBti++ = dBspline3(it + et);
307 *ptd2Bti++ = d2Bspline3(it + et);
308 }
309
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]);
317
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];
326 }
327 }
328 }
329 }
330
PutTotPVBspline3(double * Prt,double & er,double * bt,unsigned int size)331 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size)
332 {
333 #define LSIZE 12
334
335 double *bt0 = &bt[0];
336 if (er > 0.5) {
337 er = er - 1.0;
338 }
339
340 for (int ir = -1; ir <= 1; ++ir) {
341 double Br = Bspline3(-ir + er);
342 const double *btend = bt0 + size;
343 bt = bt0;
344
345 if (size >= LSIZE) {
346 btend -= LSIZE - 1;
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];
360 }
361 btend += LSIZE - 1;
362 }
363 for (; bt < btend; *Prt++ += Br * *bt++) {
364 };
365 }
366 #undef LSIZE
367 }
368
PutTotPVBspline4(double * Prt,int cr,double er,int ct,double et,int Nc,double * val,unsigned int & NbParam)369 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et, int Nc,
370 double *val, unsigned int &NbParam)
371 {
372 double Bti[4];
373 double dBti[4];
374 double d2Bti[4];
375
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);
383 }
384
385 int NbParam_ = (int)NbParam;
386
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);
390 ptBti = &Bti[0];
391 ptdBti = &dBti[0];
392 ptd2Bti = &d2Bti[0];
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);
397 *pt++ += Br_ptBti_;
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];
402 }
403 }
404 ptBti++;
405 ptdBti++;
406 ptd2Bti++;
407 }
408 }
409 }
410
PutTotPVBspline4(double * Prt,double * dPrt,double * d2Prt,int cr,double er,int ct,double et,int Ncb,double * val,unsigned int & NbParam)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)
413 {
414 double Bti[4];
415 double dBti[4];
416 double d2Bti[4];
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_ = (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
PutTotPVBspline4(double * Prt,double & er,double * bt,unsigned int size)456 void 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
PutTotPVBsplineNoSecond(double * Prt,int & cr,double & er,int & ct,double & et,int & Nc,double * val,unsigned int & NbParam,int & degree)490 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
491 double *val, unsigned int &NbParam, int °ree)
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
PutTotPVBsplineNoSecond(double * Prt,double * dPrt,int & cr,double & er,int & ct,double & et,int & Ncb,double * val,unsigned int & NbParam,int & degree)502 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
503 double &et, int &Ncb, double *val, unsigned int &NbParam,
504 int °ree)
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
PutTotPVBspline3NoSecond(double * Prt,int & cr,double & er,int & ct,double & et,int & Nc,double * val,unsigned int & NbParam)515 void 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_ = (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
PutTotPVBspline3NoSecond(double * Prt,double * dPrt,int & cr,double & er,int & ct,double & et,int & Ncb,double * val,unsigned int & NbParam)560 void 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
PutTotPVBspline4NoSecond(double * Prt,int & cr,double & er,int & ct,double & et,int & Nc,double * val,unsigned int & NbParam)610 void 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_ = (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
PutTotPVBspline4NoSecond(double * Prt,double * dPrt,int & cr,double & er,int & ct,double & et,int & Ncb,double * val,unsigned int & NbParam)639 void 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
PutTotPVBsplinePrtTout(double * PrtTout,int & cr,double & er,int & ct,double & et,int & Nc,unsigned int & NbParam,int & degree)670 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
671 int &Nc, unsigned int &NbParam, int °ree)
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
PutTotPVBspline3PrtTout(double * PrtTout,int & cr,double & er,int & ct,double & et,int & Nc,unsigned int & NbParam)682 void 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[(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 }
PutTotPVBspline4PrtTout(double * PrtTout,int & cr,double & er,int & ct,double & et,int & Nc,unsigned int & NbParam)714 void 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[(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
PutTotPVBsplinePrt(double * Prt,int & cr,double & er,int & ct,double & et,int & Ncb,unsigned int & NbParam,int & degree)735 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb,
736 unsigned int &NbParam, int °ree)
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
PutTotPVBspline3Prt(double * Prt,int & cr,double & er,int & ct,double & et,int & Ncb)747 void 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
PutTotPVBspline4Prt(double * Prt,int & cr,double & er,int & ct,double & et,int & Ncb)780 void 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
computeProbabilities(double * Prt,int & cr,double & er,int & ct,double & et,int & Nc,double * dW,unsigned int & NbParam,int & bspline,vpTemplateTrackerMI::vpHessienApproximationType & approx,bool use_hessien_des)799 void vpTemplateTrackerMIBSpline::computeProbabilities(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *dW,
800 unsigned int &NbParam, int &bspline, vpTemplateTrackerMI::vpHessienApproximationType &approx, bool use_hessien_des)
801 {
802 if (approx == vpTemplateTrackerMI::HESSIAN_NONSECOND || use_hessien_des) {
803 if (bspline==3)
804 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
805 else
806 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, dW, NbParam);
807 }
808 else {
809 if(bspline==3)
810 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, dW, NbParam);
811 else
812 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, dW, NbParam);
813 }
814 }
815
816 #endif
817