Visual Servoing Platform  version 3.2.1 under development (2019-12-07)
vpTemplateTracker.cpp
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 
41 #include <visp3/tt/vpTemplateTracker.h>
42 #include <visp3/tt/vpTemplateTrackerBSpline.h>
43 
45  : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), evolRMS(0), x_pos(), y_pos(),
46  evolRMS_eps(1e-4), ptTemplate(NULL), ptTemplatePyr(NULL), ptTemplateInit(false),
47  templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL),
48  ptTemplateSelectInit(false), templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
49  ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(),
50  Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(), HLMdesireInverse(), HLMdesireInversePyr(), G(),
51  gain(1.), thresholdGradient(40), costFunctionVerification(false), blur(true), useBrent(false), nbIterBrent(3),
52  taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0), lambdaDep(0.001),
53  iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(true), useInverse(false),
54  Warp(_warp), p(0), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
55 {
56  nbParam = Warp->getNbParam();
57  p.resize(nbParam);
58  dp.resize(nbParam);
59 
60  fgG = new double[(taillef + 1) / 2];
62 
63  fgdG = new double[(taillef + 1) / 2];
65 }
66 
67 void vpTemplateTracker::setGaussianFilterSize(unsigned int new_taill)
68 {
69  taillef = new_taill;
70  if (fgG)
71  delete[] fgG;
72  fgG = new double[taillef];
74 
75  if (fgdG)
76  delete[] fgdG;
77  fgdG = new double[taillef];
79 }
80 
82 {
83  // std::cout<<"\tInitialise reference..."<<std::endl;
84  zoneTracked = &zone;
85 
86  int largeur_im = (int)I.getWidth();
87  int hauteur_im = (int)I.getHeight();
88 
89  unsigned int NbPointDsZone = 0;
90  // double xtotal=0,ytotal=0;
91  int mod_fi, mod_fj;
92  mod_fi = mod_i;
93  mod_fj = mod_i;
94 
95  for (int i = 0; i < hauteur_im; i += mod_fi) {
96  for (int j = 0; j < largeur_im; j += mod_fj) {
97  if (zone.inZone(i, j)) {
98  NbPointDsZone++;
99  // xtotal+=j;
100  // ytotal+=i;
101  }
102  }
103  }
104 
105  // Warp->setCentre((double)xtotal/NbPointDsZone,(double)ytotal/NbPointDsZone);
106 
107  templateSize = NbPointDsZone;
109  ptTemplateInit = true;
110  ptTemplateSelect = new bool[templateSize];
111  ptTemplateSelectInit = true;
112 
115 
117  // vpTemplateTrackerZPoint ptZ;
118  vpImage<double> GaussI;
119  vpImageFilter::filter(I, GaussI, fgG, taillef);
122 
123  unsigned int cpt_point = 0;
124  templateSelectSize = 0;
125  for (int i = 0; i < hauteur_im; i += mod_i) {
126  // for(int j=minx_t;j<maxx_t;j++)
127  for (int j = 0; j < largeur_im; j += mod_j) {
128  // if(i%mod_i ==0 && j%mod_j ==0)
129  if (zone.inZone(i, j)) {
130  pt.x = j;
131  pt.y = i;
132 
133  pt.dx = dIx[i][j];
134  pt.dy = dIy[i][j];
135 
136  if (pt.dx * pt.dx + pt.dy * pt.dy > thresholdGradient) {
137  ptTemplateSelect[cpt_point] = true;
139  } else
140  ptTemplateSelect[cpt_point] = false;
141  // ptTemplate_select[cpt_point]=true;
142 
143  /*if(blur)
144  pt.val=GaussI[i][j];
145  else
146  pt.val=I[i][j];*/
147  pt.val = vpTemplateTrackerBSpline::getSubPixBspline4(GaussI, i, j);
148  // ptZone_pyr[NbLevelPyramid-cpt].push_back(pt);
149 
150  ptTemplate[cpt_point] = pt;
151  cpt_point++;
152  }
153  }
154  }
155 
156  // std::cout<<"\tNb pt template apres scale:"<<cpt_point<<std::endl;
157  // std::cout<<"utilisation de
158  // "<<taille_template_select<<"/"<<cpt_point<<" =
159  // "<<100.*taille_template_select/cpt_point<<"% pour calcul
160  // derivees"<<std::endl;
161 
162  templateSize = cpt_point;
163  GaussI.destroy();
164  // std::cout<<"\tEnd of reference initialisation ..."<<std::endl;
165 }
166 
168 {
169  // vpTRACE("destruction tracker");
170  delete[] fgG;
171  delete[] fgdG;
172 
173  resetTracker();
174 }
175 
181 {
182  // reset the tracker parameters
183  p = 0;
184 
185  // vpTRACE("resetTracking");
186  if (pyrInitialised) {
187  if (ptTemplatePyr) {
188  for (unsigned int i = 0; i < nbLvlPyr; i++) {
189  if (ptTemplatePyr[i]) {
190  for (unsigned int point = 0; point < templateSizePyr[i]; point++) {
191  delete[] ptTemplatePyr[i][point].dW;
192  delete[] ptTemplatePyr[i][point].HiG;
193  }
194  delete[] ptTemplatePyr[i];
195  }
196  }
197  delete[] ptTemplatePyr;
198  ptTemplatePyr = NULL;
199  }
200 
201  if (ptTemplateCompoPyr) {
202  for (unsigned int i = 0; i < nbLvlPyr; i++) {
203  if (ptTemplateCompoPyr[i]) {
204  for (unsigned int point = 0; point < templateSizePyr[i]; point++) {
205  delete[] ptTemplateCompoPyr[i][point].dW;
206  }
207  delete[] ptTemplateCompoPyr[i];
208  }
209  }
210  delete[] ptTemplateCompoPyr;
211  ptTemplateCompoPyr = NULL;
212  }
213 
214  if (ptTemplateSuppPyr) {
215  for (unsigned int i = 0; i < nbLvlPyr; i++) {
216  if (ptTemplateSuppPyr[i]) {
217  for (unsigned int point = 0; point < templateSizePyr[i]; point++) {
218  delete[] ptTemplateSuppPyr[i][point].Bt;
219  delete[] ptTemplateSuppPyr[i][point].BtInit;
220  delete[] ptTemplateSuppPyr[i][point].dBt;
221  delete[] ptTemplateSuppPyr[i][point].d2W;
222  delete[] ptTemplateSuppPyr[i][point].d2Wx;
223  delete[] ptTemplateSuppPyr[i][point].d2Wy;
224  }
225  delete[] ptTemplateSuppPyr[i];
226  }
227  }
228  delete[] ptTemplateSuppPyr;
229  ptTemplateSuppPyr = NULL;
230  }
231 
232  if (ptTemplateSelectPyr) {
233  for (unsigned int i = 0; i < nbLvlPyr; i++) {
234  if (ptTemplateSelectPyr[i])
235  delete[] ptTemplateSelectPyr[i];
236  }
237  delete[] ptTemplateSelectPyr;
238  ptTemplateSelectPyr = NULL;
239  }
240 
241  if (templateSizePyr) {
242  delete[] templateSizePyr;
243  templateSizePyr = NULL;
244  }
245 
246  if (HdesirePyr) {
247  delete[] HdesirePyr;
248  HdesirePyr = NULL;
249  }
250 
251  if (HLMdesirePyr) {
252  delete[] HLMdesirePyr;
253  HLMdesirePyr = NULL;
254  }
255 
256  if (HLMdesireInversePyr) {
257  delete[] HLMdesireInversePyr;
258  HLMdesireInversePyr = NULL;
259  }
260 
261  if (zoneTrackedPyr) {
262  delete[] zoneTrackedPyr;
263  zoneTrackedPyr = NULL;
264  }
265 
266  if (pyr_IDes) {
267  delete[] pyr_IDes;
268  pyr_IDes = NULL;
269  }
270  } else {
271  if (ptTemplateInit) {
272  for (unsigned int point = 0; point < templateSize; point++) {
273  delete[] ptTemplate[point].dW;
274  delete[] ptTemplate[point].HiG;
275  }
276  delete[] ptTemplate;
277  ptTemplate = NULL;
278  ptTemplateInit = false;
279  }
280  if (ptTemplateCompo) {
281  for (unsigned int point = 0; point < templateSize; point++) {
282  delete[] ptTemplateCompo[point].dW;
283  }
284  delete[] ptTemplateCompo;
285  ptTemplateCompo = NULL;
286  }
287  if (ptTemplateSupp) {
288  for (unsigned int point = 0; point < templateSize; point++) {
289  delete[] ptTemplateSupp[point].Bt;
290  delete[] ptTemplateSupp[point].BtInit;
291  delete[] ptTemplateSupp[point].dBt;
292  delete[] ptTemplateSupp[point].d2W;
293  delete[] ptTemplateSupp[point].d2Wx;
294  delete[] ptTemplateSupp[point].d2Wy;
295  }
296  delete[] ptTemplateSupp;
297  ptTemplateSupp = NULL;
298  }
299  if (ptTemplateSelectInit) {
300  if (ptTemplateSelect) {
301  delete[] ptTemplateSelect;
302  ptTemplateSelect = NULL;
303  }
304  }
305  }
306 }
307 
341 void vpTemplateTracker::display(const vpImage<unsigned char> &I, const vpColor &col, const unsigned int thickness)
342 {
343  if (I.display) { // Only if a display is associated to the image
344  vpTemplateTrackerZone zoneWarped;
345  Warp->warpZone(*zoneTracked, p, zoneWarped);
346  zoneWarped.display(I, col, thickness);
347  }
348 }
349 
383 void vpTemplateTracker::display(const vpImage<vpRGBa> &I, const vpColor &col, const unsigned int thickness)
384 {
385  if (I.display) { // Only if a display is associated to the image
386  vpTemplateTrackerZone zoneWarped;
387  Warp->warpZone(*zoneTracked, p, zoneWarped);
388  zoneWarped.display(I, col, thickness);
389  }
390 }
391 
393  vpColVector &direction, double &alpha)
394 {
395  vpColVector **ptp;
396  ptp = new vpColVector *[4];
397  vpColVector p0(nbParam);
398  p0 = tp;
399 
400  // valeur necessaire si conditionnel
401  vpColVector dpt(Warp->getNbParam());
402  vpColVector adpt(Warp->getNbParam());
403 
404  vpColVector p1(nbParam);
405  if (useCompositionnal) {
406  if (useInverse)
407  Warp->getParamInverse(direction, dpt);
408  else
409  dpt = direction;
410  Warp->pRondp(tp, dpt, p1);
411  } else {
412  p1 = tp + direction;
413  }
414 
415  vpColVector p2(nbParam);
416  if (useCompositionnal) {
417  adpt = alpha * direction;
418  if (useInverse)
419  Warp->getParamInverse(adpt, dpt);
420  else
421  dpt = adpt;
422  Warp->pRondp(tp, dpt, p2);
423  } else {
424  p2 = tp + alpha * direction;
425  }
426  vpColVector p3(nbParam);
427  ptp[0] = &p0;
428  ptp[1] = &p1;
429  ptp[2] = &p2;
430  ptp[3] = &p3;
431 
432  double *Cost = new double[4];
433  // Cost[0]=getCost(I,p0);
434  Cost[0] = tMI;
435  Cost[1] = getCost(I, p1);
436  Cost[2] = getCost(I, p2);
437 
438  double *talpha = new double[4];
439  talpha[0] = 0;
440  talpha[1] = 1.;
441  talpha[2] = alpha;
442 
443  // for(int i=0;i<3;i++)
444  // std::cout<<"alpha["<<i<<"] = "<<talpha[i]<<" Cost["<<i<<"] =
445  //"<<Cost[i]<<std::endl;
446 
447  // Utilise trois estimï¿œes de paraboles succesive ...
448  // A changer pour rendre adaptable
449  for (unsigned int opt = 0; opt < nbIterBrent; opt++) {
450  // double a=talpha[0];
451  // double b=talpha[1];
452  // double c=talpha[2];
453  // double Cost0=Cost[0];
454  // double Cost1=Cost[1];
455  // double Cost2=Cost[2];
456 
457  vpMatrix A(3, 3);
458  for (unsigned int i = 0; i < 3; i++) {
459  A[i][0] = talpha[i] * talpha[i];
460  A[i][1] = talpha[i];
461  A[i][2] = 1.;
462  }
463  // std::cout<<"A="<<A<<std::endl;
464  vpColVector B(3);
465  for (unsigned int i = 0; i < 3; i++)
466  B[i] = Cost[i];
467  vpColVector parabol(3);
468  parabol = (A.t() * A).inverseByLU() * A.t() * B;
469  // vpColVector parabol(3);parabol=A.pseudoInverse()*B;
470 
471  // si convexe
472  if (parabol[0] > 0) {
473  talpha[3] = -0.5 * parabol[1] / parabol[0];
474  // std::cout<<"parabol = "<<parabol<<std::endl;
475  // std::cout<<"convexe talpha = "<<talpha[3]<<std::endl;
476  } else // si concave
477  {
478  int tindic_x_min = 0;
479  int tindic_x_max = 0;
480  for (int i = 1; i < 3; i++) {
481  if (talpha[i] < talpha[tindic_x_min])
482  tindic_x_min = i;
483  if (talpha[i] > talpha[tindic_x_max])
484  tindic_x_max = i;
485  }
486 
487  if (Cost[tindic_x_max] < Cost[tindic_x_min]) {
488  // talpha[3]=talpha[tindic_x_max]+1.;
489  talpha[3] = talpha[tindic_x_max] + 1.;
490  /*if(talpha[tindic_x_min]>talpha[tindic_x_max])
491  talpha[3]=talpha[tindic_x_min]+1.;
492  else
493  talpha[3]=talpha[tindic_x_min]-1.;*/
494  } else {
495  // talpha[3]=talpha[tindic_x_min]-1.;
496  talpha[3] = talpha[tindic_x_min] - 1.;
497  /*if(talpha[tindic_x_min]<talpha[tindic_x_max])
498  talpha[3]=talpha[tindic_x_max]+1.;
499  else
500  talpha[3]=talpha[tindic_x_max]-1.;*/
501  }
502  // std::cout<<"concave talpha="<<talpha[3]<<std::endl;
503  }
504  // std::cout<<"talpha="<<talpha[3]<<std::endl;
505  int indic_x_min = 0;
506  int indic_x_max = 0;
507  for (int i = 1; i < 3; i++) {
508  if (talpha[i] < talpha[indic_x_min])
509  indic_x_min = i;
510  if (talpha[i] > talpha[indic_x_max])
511  indic_x_max = i;
512  }
513  // std::cout<<"talpha = "<<talpha[3]<<std::endl;
514  if (talpha[3] > talpha[indic_x_max])
515  if ((talpha[3] - talpha[indic_x_max]) > alpha)
516  talpha[3] = talpha[indic_x_max] + 4.;
517  if (talpha[3] < talpha[indic_x_min])
518  if ((talpha[indic_x_min] - talpha[3]) > alpha)
519  talpha[3] = talpha[indic_x_min] - 4.;
520 
521  /*if(((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0))==0)
522  {Cost[3]=1000;break;}
523 
524  //calcul du gain correspondant au minimum de la parabole estimï¿œe
525  talpha[3]=b-0.5*((b-a)*(b-a)*(Cost1-Cost2)-(b-c)*(b-c)*(Cost1-Cost0))/((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0));
526  int indic_x_min=0;
527  int indic_x_max=0;
528  for(int i=1;i<3;i++)
529  {
530  if(talpha[i]<talpha[indic_x_min])
531  indic_x_min=i;
532  if(talpha[i]>talpha[indic_x_max])
533  indic_x_max=i;
534  }
535  std::cout<<"talpha = "<<talpha[3]<<std::endl;
536  if(talpha[3]>talpha[indic_x_max])
537  if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+alpha;
538  if(talpha[3]<talpha[indic_x_min])
539  if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-alpha;*/
540 
541  // p3=tp-talpha[3]*direction;
542  if (useCompositionnal) {
543  adpt = talpha[3] * direction;
544  if (useInverse)
545  Warp->getParamInverse(adpt, dpt);
546  else
547  dpt = adpt;
548  Warp->pRondp(tp, dpt, p3);
549  } else {
550  p3 = tp + talpha[3] * direction;
551  }
552 
553  Cost[3] = getCost(I, p3);
554  // std::cout<<"new cost="<<Cost[3]<<std::endl;
555 
556  int indice_f_max = 0;
557  for (int i = 1; i < 4; i++)
558  if (Cost[i] > Cost[indice_f_max])
559  indice_f_max = i;
560  if (indice_f_max != 3) {
561  *ptp[indice_f_max] = *ptp[3];
562  Cost[indice_f_max] = Cost[3];
563  talpha[indice_f_max] = talpha[3];
564  } else
565  break;
566  }
567 
568  int indice_f_min = 0;
569  for (int i = 0; i < 4; i++)
570  if (Cost[i] < Cost[indice_f_min])
571  indice_f_min = i;
572 
573  alpha = talpha[indice_f_min];
574 
575  if (alpha < 1)
576  alpha = 1.;
577 
578  delete[] ptp;
579  delete[] Cost;
580  delete[] talpha;
581 }
582 
588 void vpTemplateTracker::initPyramidal(unsigned int nbLvl, unsigned int l0)
589 {
590  // vpTRACE("init_pyramidal");
591  nbLvlPyr = nbLvl;
592  l0Pyr = l0;
593 
597  ptTemplateSelectPyr = new bool *[nbLvlPyr];
598  ptTemplateSuppPyr = new vpTemplateTrackerPointSuppMIInv *[nbLvlPyr];
600  for (unsigned int i = 0; i < nbLvlPyr; i++) {
601  ptTemplatePyr[i] = NULL;
602  ptTemplateSuppPyr[i] = NULL;
603  ptTemplateSelectPyr[i] = NULL;
604  ptTemplateCompoPyr[i] = NULL;
605  }
606  templateSizePyr = new unsigned int[nbLvlPyr];
610 
611  pyrInitialised = true;
612  // vpTRACE("fin init_pyramidal");
613 }
615 {
616  // vpTRACE("initTrackingPyr");
617  zoneTrackedPyr[0].copy(zone);
618  // vpTRACE("fin copy zone");
619 
620  pyr_IDes[0] = I;
625 
626  // creationpyramide de zones et images desiree
627  if (nbLvlPyr > 1) {
628  for (unsigned int i = 1; i < nbLvlPyr; i++) {
631 
636  // reste probleme avec le Hessien
637  }
638  }
639  /*for(int i=0;i<nbLvlPyr;i++)
640  {
641  vpColVector ptemp(8);ptemp=0;
642  zoneTracked=&zoneTrackedPyr[i];
643  init_pos_evalRMS(ptemp);
644  }*/
646 
647  // vpTRACE("fin initTrackingPyr");
648 }
649 
672 {
673  zoneRef_.initClick(I, delaunay);
674 
675  if (nbLvlPyr > 1) {
679  } else {
682  // trackNoPyr(I);
683  }
684 }
685 
697 void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &v_ip,
698  bool delaunay)
699 {
700  zoneRef_.initFromPoints(I, v_ip, delaunay);
701 
702  if (nbLvlPyr > 1) {
706  } else {
709  // trackNoPyr(I);
710  }
711 }
712 
720 {
721  zoneRef_ = zone;
722 
723  if (nbLvlPyr > 1) {
727  } else {
730  // trackNoPyr(I);
731  }
732 }
733 
735 {
736  // vpTRACE("initHessienDesiredPyr");
737 
739  // ptTemplateSupp=ptTemplateSuppPyr[0];
740  // ptTemplateCompo=ptTemplateCompoPyr[0];
743  // ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
744  try {
746  ptTemplateSuppPyr[0] = ptTemplateSupp;
748  HdesirePyr[0] = Hdesire;
749  HLMdesirePyr[0] = HLMdesire;
751  } catch (const vpException &e) {
752  ptTemplateSuppPyr[0] = ptTemplateSupp;
754  HdesirePyr[0] = Hdesire;
755  HLMdesirePyr[0] = HLMdesire;
757  throw(e);
758  }
759 
760  if (nbLvlPyr > 1) {
762  Itemp = I;
763  for (unsigned int i = 1; i < nbLvlPyr; i++) {
764  vpImageFilter::getGaussPyramidal(Itemp, Itemp);
765 
769  // ptTemplateSupp=ptTemplateSuppPyr[i];
770  // ptTemplateCompo=ptTemplateCompoPyr[i];
771  try {
772  initHessienDesired(Itemp);
773  ptTemplateSuppPyr[i] = ptTemplateSupp;
775  HdesirePyr[i] = Hdesire;
776  HLMdesirePyr[i] = HLMdesire;
778  } catch (const vpException &e) {
779  ptTemplateSuppPyr[i] = ptTemplateSupp;
781  HdesirePyr[i] = Hdesire;
782  HLMdesirePyr[i] = HLMdesire;
784  throw(e);
785  }
786  }
787  }
788  // vpTRACE("fin initHessienDesiredPyr");
789 }
790 
796 {
797  if (nbLvlPyr > 1)
798  trackPyr(I);
799  else
800  trackNoPyr(I);
801 }
802 
804 {
805  // vpTRACE("trackPyr");
806  vpImage<unsigned char> *pyr_I;
807  // pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
808  pyr_I = new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
809  pyr_I[0] = I;
810 
811  try {
812  vpColVector ptemp(nbParam);
813  if (nbLvlPyr > 1) {
814  // vpColVector *p_sauv=new vpColVector[nbLvlPyr];
815  // for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);
816 
817  // p_sauv[0]=p;
818  for (unsigned int i = 1; i < nbLvlPyr; i++) {
819  vpImageFilter::getGaussPyramidal(pyr_I[i - 1], pyr_I[i]);
820  // test getParamPyramidDown
821  /*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
822  vpColVector vX_test2(2);
823  Warp->computeCoeff(p);
824  Warp->computeDenom(vX_test,p);
825  Warp->warpX(vX_test,vX_test2,p);
826  std::cout<<"p = "<<p.t()<<std::endl;*/
827  // std::cout<<"get p down"<<std::endl;
828  Warp->getParamPyramidDown(p, ptemp);
829  p = ptemp;
831 
832  // p_sauv[i]=p;
833  /*std::cout<<"p_down = "<<p.t()<<std::endl;
834 
835  vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
836  vpColVector vX_testd2(2);
837  Warp->computeCoeff(p);
838  Warp->computeDenom(vX_testd,p);
839  Warp->warpX(vX_testd,vX_testd2,p);
840  std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=>
841  "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/
842  }
843 
844  for (int i = (int)nbLvlPyr - 1; i >= 0; i--) {
845  if (i >= (int)l0Pyr) {
849  ptTemplateSupp = ptTemplateSuppPyr[i];
851  H = HdesirePyr[i];
852  HLM = HLMdesirePyr[i];
854  // zoneTracked=&zoneTrackedPyr[i];
855  trackRobust(pyr_I[i]);
856  }
857  // std::cout<<"get p up"<<std::endl;
858  // ptemp=p_sauv[i-1];
859  if (i > 0) {
860  Warp->getParamPyramidUp(p, ptemp);
861  p = ptemp;
862  zoneTracked = &zoneTrackedPyr[i - 1];
863  }
864  }
865 #if 0
866  if(l0Pyr==0)
867  {
871  ptTemplateSupp=ptTemplateSuppPyr[0];
873  H=HdesirePyr[0];
874  HLM=HLMdesirePyr[0];
877  trackRobust(pyr_I[0]);
878  }
879 
880  if (l0Pyr > 0) {
881  // for (int l=(int)l0Pyr; l >=0; l--) {
882  // Warp->getParamPyramidUp(p,ptemp);
883  // p=ptemp;
884  // }
886  }
887 #endif
888  // delete [] p_sauv;
889  } else {
890  // std::cout<<"reviens a tracker de base"<<std::endl;
891  trackRobust(I);
892  }
893  delete[] pyr_I;
894  } catch (const vpException &e) {
895  delete[] pyr_I;
897  }
898 }
899 
901 {
903  vpColVector p_pre_estimation;
904  p_pre_estimation = p;
906  double pre_fcost = getCost(I, p);
907 
908  trackNoPyr(I);
909 
910  // std::cout<<"fct avant : "<<pre_fcost<<std::endl;
911  double post_fcost = getCost(I, p);
912  // std::cout<<"fct apres : "<<post_fcost<<std::endl<<std::endl;
913  if (pre_fcost < post_fcost)
914  p = p_pre_estimation;
915  } else
916  trackNoPyr(I);
917 }
918 
926 {
927  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
928 
929  Warp->computeCoeff(p);
930  evolRMS = 0;
931  vpTemplateTrackerTriangle triangle;
932 
933  for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
934  zoneTracked->getTriangle(i, triangle);
935  for (unsigned int j = 0; j < 3; j++) {
936  triangle.getCorner(j, X1[0], X1[1]);
937 
938  Warp->computeDenom(X1, p);
939  Warp->warpX(X1, X2, p);
940 
941  unsigned int index = i * 3 + j;
942  double x_ = x_pos[index] - X2[0];
943  double y_ = y_pos[index] - X2[1];
944  evolRMS += x_ * x_ + y_ * y_;
945  x_pos[index] = X2[0];
946  y_pos[index] = X2[1];
947  }
948  }
949  evolRMS /= nb_corners;
950 }
951 
959 {
960  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
961  x_pos.resize(nb_corners);
962  y_pos.resize(nb_corners);
963 
964  Warp->computeCoeff(p);
965  vpTemplateTrackerTriangle triangle;
966 
967  for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
968  unsigned int i3 = i * 3;
969  zoneTracked->getTriangle(i, triangle);
970  for (unsigned int j = 0; j < 3; j++) {
971  triangle.getCorner(j, X1[0], X1[1]);
972 
973  Warp->computeDenom(X1, p);
974  Warp->warpX(X1, X2, p);
975  x_pos[i3 + j] = X2[0];
976  y_pos[i3 + j] = X2[1];
977  }
978  }
979 }
static void getGaussPyramidal(const vpImage< unsigned char > &I, vpImage< unsigned char > &GI)
vpTemplateTrackerZone getPyramidDown() const
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
vpDisplay * display
Definition: vpImage.h:142
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
virtual void getParamPyramidDown(const vpColVector &p, vpColVector &pdown)=0
virtual void trackPyr(const vpImage< unsigned char > &I)
unsigned int templateSelectSize
void getGaussianBluredImage(const vpImage< unsigned char > &I)
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerPoint ** ptTemplatePyr
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
unsigned int getNbParam() const
static void getGaussianKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true)
virtual void initTrackingPyr(const vpImage< unsigned char > &I, vpTemplateTrackerZone &zone)
std::vector< double > y_pos
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
error that can be emited by ViSP classes.
Definition: vpException.h:71
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
void initPosEvalRMS(const vpColVector &p)
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
void computeEvalRMS(const vpColVector &p)
vpMatrix * HLMdesireInversePyr
vpTemplateTracker()
Default constructor.
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
virtual void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const =0
void copy(const vpTemplateTrackerZone &z)
unsigned int templateSize
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
void initTracking(const vpImage< unsigned char > &I, vpTemplateTrackerZone &zone)
vpMatrix t() const
Definition: vpMatrix.cpp:507
bool inZone(const int &i, const int &j) const
Error that can be emited by the vpTracker class and its derivates.
std::vector< double > x_pos
vpTemplateTrackerZone * zoneTrackedPyr
virtual void initHessienDesired(const vpImage< unsigned char > &I)=0
vpTemplateTrackerZone zoneRef_
vpImage< double > dIx
void setGaussianFilterSize(unsigned int new_taill)
void initFromZone(const vpImage< unsigned char > &I, const vpTemplateTrackerZone &zone)
virtual double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)=0
virtual void initHessienDesiredPyr(const vpImage< unsigned char > &I)
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
vpImage< double > dIy
virtual void getParamPyramidUp(const vpColVector &p, vpColVector &pup)=0
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &ip, bool delaunay=false)
void destroy()
Destructor : Memory de-allocation.
Definition: vpImage.h:908
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &v_ip, bool delaunay=false)
vpColVector getCorner(unsigned int i) const
const char * getMessage(void) const
Definition: vpException.cpp:90
unsigned int * templateSizePyr
unsigned int nbLvlPyr
virtual void initPyramidal(unsigned int nbLvl, unsigned int l0)
void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out)
static void getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true)
unsigned int getHeight() const
Definition: vpImage.h:186
vpTemplateTrackerPointCompo * ptTemplateCompo
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void track(const vpImage< unsigned char > &I)
virtual void trackNoPyr(const vpImage< unsigned char > &I)=0
vpTemplateTrackerZone * zoneTracked
unsigned int getNbTriangle() const
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
void trackRobust(const vpImage< unsigned char > &I)
vpImage< unsigned char > * pyr_IDes
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
vpTemplateTrackerWarp * Warp
unsigned int getWidth() const
Definition: vpImage.h:244
unsigned int nbIterBrent
vpTemplateTrackerPointCompo ** ptTemplateCompoPyr
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)