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