Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpTemplateTracker.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Template tracker.
32  *
33  * Authors:
34  * Amaury Dame
35  * Aurelien Yol
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <visp3/tt/vpTemplateTracker.h>
41 #include <visp3/tt/vpTemplateTrackerBSpline.h>
42 
44  : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
45  ptTemplateInit(false), templateSize(0), templateSizePyr(NULL),
46  ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false),
47  templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
48  ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL),
49  pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(),
50  HLMdesireInverse(), HLMdesireInversePyr(), G(), gain(1.), thresholdGradient(40),
51  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),
53  lambdaDep(0.001), iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0),
54  useCompositionnal(true), useInverse(false), Warp(_warp), p(0), dp(), X1(), X2(),
55  dW(), BI(), dIx(), dIy(), zoneRef_()
56 {
57  nbParam = Warp->getNbParam() ;
58  p.resize(nbParam);
59  dp.resize(nbParam);
60 
61  fgG=new double[(taillef+1)/2] ;
63 
64  fgdG=new double[(taillef+1)/2] ;
66 }
67 
68 void vpTemplateTracker::setGaussianFilterSize(unsigned int new_taill)
69 {
70  taillef=new_taill;
71  if (fgG) delete[] fgG;
72  fgG=new double[taillef] ;
74 
75  if (fgdG) 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;mod_fj=mod_i;
92 
93  for(int i=0;i<hauteur_im;i+=mod_fi) {
94  for(int j=0;j<largeur_im;j+=mod_fj) {
95  if(zone.inZone(i,j)) {
96  NbPointDsZone++;
97  //xtotal+=j;
98  //ytotal+=i;
99  }
100  }
101  }
102 
103  //Warp->setCentre((double)xtotal/NbPointDsZone,(double)ytotal/NbPointDsZone);
104 
105  templateSize=NbPointDsZone;
108 
111 
113  //vpTemplateTrackerZPoint ptZ;
114  vpImage<double> GaussI ;
115  vpImageFilter::filter(I, GaussI,fgG,taillef);
118 
119  unsigned int cpt_point=0;
121  for(int i=0;i<hauteur_im;i+=mod_i)
122  {
123  //for(int j=minx_t;j<maxx_t;j++)
124  for(int j=0;j<largeur_im;j+=mod_j)
125  {
126 // if(i%mod_i ==0 && j%mod_j ==0)
127  if(zone.inZone(i,j))
128  {
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  {
137  ptTemplateSelect[cpt_point]=true;
139  }
140  else
141  ptTemplateSelect[cpt_point]=false;
142  //ptTemplate_select[cpt_point]=true;
143 
144  /*if(blur)
145  pt.val=GaussI[i][j];
146  else
147  pt.val=I[i][j];*/
148  pt.val=vpTemplateTrackerBSpline::getSubPixBspline4(GaussI,i,j);
149  //ptZone_pyr[NbLevelPyramid-cpt].push_back(pt);
150 
151  ptTemplate[cpt_point]=pt;
152  cpt_point++;
153  }
154  }
155  }
156 
157  // std::cout<<"\tNb pt template apres scale:"<<cpt_point<<std::endl;
158  // std::cout<<"utilisation de "<<taille_template_select<<"/"<<cpt_point<<" = "<<100.*taille_template_select/cpt_point<<"% pour calcul derivees"<<std::endl;
159 
160  templateSize=cpt_point;
161  GaussI.destroy();
162  // std::cout<<"\tEnd of reference initialisation ..."<<std::endl;
163 }
164 
166 {
167  // vpTRACE("destruction tracker");
168  delete[] fgG;
169  delete[] fgdG;
170 
171  resetTracker();
172 }
173 
178 {
179  // reset the tracker parameters
180  p = 0;
181 
182  // vpTRACE("resetTracking");
183  if(pyrInitialised)
184  {
185  if(ptTemplatePyr){
186  for(unsigned int i=0;i<nbLvlPyr;i++)
187  {
188  if(ptTemplatePyr[i]){
189  for(unsigned int point=0;point<templateSizePyr[i];point++)
190  {
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  {
204  if (ptTemplateCompoPyr[i]) {
205  for(unsigned int point=0;point<templateSizePyr[i];point++) {
206  delete[] ptTemplateCompoPyr[i][point].dW;
207  }
208  delete[] ptTemplateCompoPyr[i];
209  }
210  }
211  delete[] ptTemplateCompoPyr;
212  ptTemplateCompoPyr = NULL;
213  }
214 
215  if (ptTemplateSuppPyr) {
216  for(unsigned int i=0;i<nbLvlPyr;i++)
217  {
218  if (ptTemplateSuppPyr[i]) {
219  for(unsigned int point=0;point<templateSizePyr[i];point++) {
220  delete[] ptTemplateSuppPyr[i][point].Bt;
221  delete[] ptTemplateSuppPyr[i][point].BtInit;
222  delete[] ptTemplateSuppPyr[i][point].dBt;
223  delete[] ptTemplateSuppPyr[i][point].d2W;
224  delete[] ptTemplateSuppPyr[i][point].d2Wx;
225  delete[] ptTemplateSuppPyr[i][point].d2Wy;
226  }
227  delete[] ptTemplateSuppPyr[i];
228  }
229  }
230  delete[] ptTemplateSuppPyr;
231  ptTemplateSuppPyr = NULL;
232  }
233 
235  for(unsigned int i=0;i<nbLvlPyr;i++){
236  if(ptTemplateSelectPyr[i])
237  delete[] ptTemplateSelectPyr[i];
238  }
239  delete[] ptTemplateSelectPyr;
240  ptTemplateSelectPyr = NULL;
241  }
242 
243  if(templateSizePyr){
244  delete[] templateSizePyr;
245  templateSizePyr = NULL;
246  }
247 
248  if(HdesirePyr) {
249  delete[] HdesirePyr;
250  HdesirePyr = NULL;
251  }
252 
253  if(HLMdesirePyr){
254  delete[] HLMdesirePyr;
255  HLMdesirePyr = NULL;
256  }
257 
259  delete[] HLMdesireInversePyr;
260  HLMdesireInversePyr = NULL;
261  }
262 
263  if(zoneTrackedPyr){
264  delete[] zoneTrackedPyr;
265  zoneTrackedPyr = NULL;
266  }
267 
268  if(pyr_IDes){
269  delete[] pyr_IDes;
270  pyr_IDes = NULL;
271  }
272  }
273  else
274  {
275  if(ptTemplateInit)
276  {
277  for(unsigned int point=0;point<templateSize;point++)
278  {
279  delete[] ptTemplate[point].dW;
280  delete[] ptTemplate[point].HiG;
281  }
282  delete[] ptTemplate;
283  ptTemplate = NULL;
284  ptTemplateInit = false;
285  }
286  if (ptTemplateCompo) {
287  for(unsigned int point=0;point<templateSize;point++)
288  {
289  delete[] ptTemplateCompo[point].dW;
290  }
291  delete[] ptTemplateCompo;
292  ptTemplateCompo = NULL;
293  }
294  if (ptTemplateSupp) {
295  for(unsigned int point=0;point<templateSize;point++)
296  {
297  delete[] ptTemplateSupp[point].Bt;
298  delete[] ptTemplateSupp[point].BtInit;
299  delete[] ptTemplateSupp[point].dBt;
300  delete[] ptTemplateSupp[point].d2W;
301  delete[] ptTemplateSupp[point].d2Wx;
302  delete[] ptTemplateSupp[point].d2Wy;
303  }
304  delete[] ptTemplateSupp;
305  ptTemplateSupp = NULL;
306  }
308  if (ptTemplateSelect) {
309  delete[] ptTemplateSelect; ptTemplateSelect = NULL;
310  }
311  }
312  }
313 }
314 
348 void vpTemplateTracker::display(const vpImage<unsigned char> &I, const vpColor& col, const unsigned int thickness)
349 {
350  if (I.display) { // Only if a display is associated to the image
351  vpTemplateTrackerZone zoneWarped;
352  Warp->warpZone(*zoneTracked, p, zoneWarped);
353  zoneWarped.display(I, col, thickness);
354  }
355 }
356 
390 void vpTemplateTracker::display(const vpImage<vpRGBa> &I, const vpColor& col, const unsigned int thickness)
391 {
392  if (I.display) { // Only if a display is associated to the image
393  vpTemplateTrackerZone zoneWarped;
394  Warp->warpZone(*zoneTracked, p, zoneWarped);
395  zoneWarped.display(I, col, thickness);
396  }
397 }
398 
399 void vpTemplateTracker::computeOptimalBrentGain(const vpImage<unsigned char> &I,vpColVector &tp,double tMI,vpColVector &direction,double &alpha)
400 {
401  vpColVector **ptp;
402  ptp=new vpColVector*[4];
403  vpColVector p0(nbParam);
404  p0=tp;
405 
406  //valeur necessaire si conditionnel
407  vpColVector dpt(Warp->getNbParam());
408  vpColVector adpt(Warp->getNbParam());
409 
410  vpColVector p1(nbParam);
412  {
413  if(useInverse)
414  Warp->getParamInverse(direction,dpt);
415  else
416  dpt=direction;
417  Warp->pRondp(tp,dpt,p1);
418  }
419  else
420  {
421  p1=tp+direction;
422  }
423 
424  vpColVector p2(nbParam);
426  {
427  adpt=alpha*direction;
428  if(useInverse)
429  Warp->getParamInverse(adpt,dpt);
430  else
431  dpt=adpt;
432  Warp->pRondp(tp,dpt,p2);
433  }
434  else
435  {
436  p2=tp+alpha*direction;
437  }
438  vpColVector p3(nbParam);
439  ptp[0]=&p0;
440  ptp[1]=&p1;
441  ptp[2]=&p2;
442  ptp[3]=&p3;
443 
444 
445  double *Cost=new double[4];
446  //Cost[0]=getCost(I,p0);
447  Cost[0]=tMI;
448  Cost[1]=getCost(I,p1);
449  Cost[2]=getCost(I,p2);
450 
451 
452  double *talpha=new double[4];
453  talpha[0]=0;
454  talpha[1]=1.;
455  talpha[2]=alpha;
456 
457  //for(int i=0;i<3;i++)
458  // std::cout<<"alpha["<<i<<"] = "<<talpha[i]<<" Cost["<<i<<"] = "<<Cost[i]<<std::endl;
459 
460  //Utilise trois estimï¿œes de paraboles succesive ...
461  //A changer pour rendre adaptable
462  for(unsigned int opt=0;opt<nbIterBrent;opt++)
463  {
464  //double a=talpha[0];
465  //double b=talpha[1];
466  //double c=talpha[2];
467  //double Cost0=Cost[0];
468  //double Cost1=Cost[1];
469  //double Cost2=Cost[2];
470 
471  vpMatrix A(3,3);
472  for(unsigned int i=0;i<3;i++){A[i][0]=talpha[i]*talpha[i];A[i][1]=talpha[i];A[i][2]=1.;}
473  //std::cout<<"A="<<A<<std::endl;
474  vpColVector B(3);for(unsigned int i=0;i<3;i++)B[i]=Cost[i];
475  vpColVector parabol(3);parabol=(A.t()*A).inverseByLU()*A.t()*B;
476  //vpColVector parabol(3);parabol=A.pseudoInverse()*B;
477 
478  //si convexe
479  if(parabol[0]>0)
480  {
481  talpha[3]=-0.5*parabol[1]/parabol[0];
482  //std::cout<<"parabol = "<<parabol<<std::endl;
483  //std::cout<<"convexe talpha = "<<talpha[3]<<std::endl;
484  }
485  else//si concave
486  {
487  int tindic_x_min=0;
488  int tindic_x_max=0;
489  for(int i=1;i<3;i++)
490  {
491  if(talpha[i]<talpha[tindic_x_min])
492  tindic_x_min=i;
493  if(talpha[i]>talpha[tindic_x_max])
494  tindic_x_max=i;
495  }
496 
497  if(Cost[tindic_x_max]<Cost[tindic_x_min])
498  {
499  //talpha[3]=talpha[tindic_x_max]+1.;
500  talpha[3]=talpha[tindic_x_max]+1.;
501  /*if(talpha[tindic_x_min]>talpha[tindic_x_max])
502  talpha[3]=talpha[tindic_x_min]+1.;
503  else
504  talpha[3]=talpha[tindic_x_min]-1.;*/
505  }
506  else
507  {
508  //talpha[3]=talpha[tindic_x_min]-1.;
509  talpha[3]=talpha[tindic_x_min]-1.;
510  /*if(talpha[tindic_x_min]<talpha[tindic_x_max])
511  talpha[3]=talpha[tindic_x_max]+1.;
512  else
513  talpha[3]=talpha[tindic_x_max]-1.;*/
514  }
515  //std::cout<<"concave talpha="<<talpha[3]<<std::endl;
516  }
517  //std::cout<<"talpha="<<talpha[3]<<std::endl;
518  int indic_x_min=0;
519  int indic_x_max=0;
520  for(int i=1;i<3;i++)
521  {
522  if(talpha[i]<talpha[indic_x_min])
523  indic_x_min=i;
524  if(talpha[i]>talpha[indic_x_max])
525  indic_x_max=i;
526  }
527  //std::cout<<"talpha = "<<talpha[3]<<std::endl;
528  if(talpha[3]>talpha[indic_x_max])
529  if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+4.;
530  if(talpha[3]<talpha[indic_x_min])
531  if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-4.;
532 
533  /*if(((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0))==0)
534  {Cost[3]=1000;break;}
535 
536  //calcul du gain correspondant au minimum de la parabole estimï¿œe
537  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));
538  int indic_x_min=0;
539  int indic_x_max=0;
540  for(int i=1;i<3;i++)
541  {
542  if(talpha[i]<talpha[indic_x_min])
543  indic_x_min=i;
544  if(talpha[i]>talpha[indic_x_max])
545  indic_x_max=i;
546  }
547  std::cout<<"talpha = "<<talpha[3]<<std::endl;
548  if(talpha[3]>talpha[indic_x_max])
549  if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+alpha;
550  if(talpha[3]<talpha[indic_x_min])
551  if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-alpha;*/
552 
553  //p3=tp-talpha[3]*direction;
555  {
556  adpt=talpha[3]*direction;
557  if(useInverse)
558  Warp->getParamInverse(adpt,dpt);
559  else
560  dpt=adpt;
561  Warp->pRondp(tp,dpt,p3);
562  }
563  else
564  {
565  p3=tp+talpha[3]*direction;
566  }
567 
568  Cost[3]=getCost(I,p3);
569  //std::cout<<"new cost="<<Cost[3]<<std::endl;
570 
571  int indice_f_max=0;
572  for(int i=1;i<4;i++)
573  if(Cost[i]>Cost[indice_f_max])
574  indice_f_max=i;
575  if(indice_f_max!=3)
576  {
577  *ptp[indice_f_max]=*ptp[3];
578  Cost[indice_f_max]=Cost[3];
579  talpha[indice_f_max]=talpha[3];
580  }
581  else
582  break;
583  }
584 
585  int indice_f_min=0;
586  for(int i=0;i<4;i++)
587  if(Cost[i]<Cost[indice_f_min])
588  indice_f_min=i;
589 
590  alpha=talpha[indice_f_min];
591 
592  if(alpha<1)alpha=1.;
593 
594  delete[] ptp;
595  delete[] Cost;
596  delete[] talpha;
597 }
598 
599 
604 void vpTemplateTracker::initPyramidal(unsigned int nbLvl, unsigned int l0)
605 {
606  // vpTRACE("init_pyramidal");
607  nbLvlPyr=nbLvl;
608  l0Pyr=l0;
609 
613  ptTemplateSelectPyr=new bool*[nbLvlPyr];
614  ptTemplateSuppPyr=new vpTemplateTrackerPointSuppMIInv*[nbLvlPyr];
616  for(unsigned int i=0; i< nbLvlPyr; i++) {
617  ptTemplatePyr[i] = NULL;
618  ptTemplateSuppPyr[i] = NULL;
619  ptTemplateSelectPyr[i] = NULL;
620  ptTemplateCompoPyr[i] = NULL;
621  }
622  templateSizePyr=new unsigned int[nbLvlPyr];
626 
627  pyrInitialised=true;
628  // vpTRACE("fin init_pyramidal");
629 
630 }
632 {
633  // vpTRACE("initTrackingPyr");
634  zoneTrackedPyr[0].copy(zone);
635  //vpTRACE("fin copy zone");
636 
637  pyr_IDes[0]=I;
642 
643  //creationpyramide de zones et images desiree
644  if(nbLvlPyr>1)
645  {
646  for(unsigned int i=1;i<nbLvlPyr;i++)
647  {
650 
655  //reste probleme avec le Hessien
656  }
657  }
658  /*for(int i=0;i<nbLvlPyr;i++)
659  {
660  vpColVector ptemp(8);ptemp=0;
661  zoneTracked=&zoneTrackedPyr[i];
662  init_pos_evalRMS(ptemp);
663  }*/
665 
666  // vpTRACE("fin initTrackingPyr");
667 
668 }
669 
689 {
690  zoneRef_.initClick(I, delaunay);
691 
692  if (nbLvlPyr > 1) {
696  }
697  else {
700 // trackNoPyr(I);
701  }
702 }
703 
714 void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector< vpImagePoint > &v_ip, bool delaunay)
715 {
716  zoneRef_.initFromPoints(I, v_ip, delaunay);
717 
718  if (nbLvlPyr > 1) {
722  }
723  else {
726  //trackNoPyr(I);
727  }
728 }
729 
737 {
738  zoneRef_ = zone;
739 
740  if (nbLvlPyr > 1) {
744  }
745  else {
748 // trackNoPyr(I);
749  }
750 }
751 
753 {
754  // vpTRACE("initHessienDesiredPyr");
755 
757  //ptTemplateSupp=ptTemplateSuppPyr[0];
758  //ptTemplateCompo=ptTemplateCompoPyr[0];
761 // ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
762  try{
764  ptTemplateSuppPyr[0]=ptTemplateSupp;
766  HdesirePyr[0]=Hdesire;
769  }
770  catch(vpException &e){
771  ptTemplateSuppPyr[0]=ptTemplateSupp;
773  HdesirePyr[0]=Hdesire;
776  throw(e);
777  }
778 
779  if(nbLvlPyr>1)
780  {
781  vpImage<unsigned char> Itemp;Itemp=I;
782  for(unsigned int i=1;i<nbLvlPyr;i++)
783  {
785 
789  //ptTemplateSupp=ptTemplateSuppPyr[i];
790  //ptTemplateCompo=ptTemplateCompoPyr[i];
791  try{
792  initHessienDesired(Itemp);
793  ptTemplateSuppPyr[i]=ptTemplateSupp;
795  HdesirePyr[i]=Hdesire;
798  }
799  catch(vpException &e){
800  ptTemplateSuppPyr[i]=ptTemplateSupp;
802  HdesirePyr[i]=Hdesire;
805  throw(e);
806  }
807  }
808  }
809  // vpTRACE("fin initHessienDesiredPyr");
810 }
811 
817 {
818  if (nbLvlPyr > 1)
819  trackPyr(I);
820  else
821  trackNoPyr(I);
822 }
823 
825 {
826  //vpTRACE("trackPyr");
827  vpImage<unsigned char> *pyr_I;
828 // pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
829  pyr_I=new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
830  pyr_I[0]=I;
831 
832  try
833  {
834  vpColVector ptemp(nbParam);
835  if(nbLvlPyr>1)
836  {
837  // vpColVector *p_sauv=new vpColVector[nbLvlPyr];
838  // for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);
839 
840  // p_sauv[0]=p;
841  for(unsigned int i=1;i<nbLvlPyr;i++)
842  {
843  vpImageFilter::getGaussPyramidal(pyr_I[i-1],pyr_I[i]);
844  //test getParamPyramidDown
845  /*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
846  vpColVector vX_test2(2);
847  Warp->computeCoeff(p);
848  Warp->computeDenom(vX_test,p);
849  Warp->warpX(vX_test,vX_test2,p);
850  std::cout<<"p = "<<p.t()<<std::endl;*/
851  //std::cout<<"get p down"<<std::endl;
852  Warp->getParamPyramidDown(p,ptemp);
853  p=ptemp;
855 
856  // p_sauv[i]=p;
857  /*std::cout<<"p_down = "<<p.t()<<std::endl;
858 
859  vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
860  vpColVector vX_testd2(2);
861  Warp->computeCoeff(p);
862  Warp->computeDenom(vX_testd,p);
863  Warp->warpX(vX_testd,vX_testd2,p);
864  std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=> "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/
865 
866  }
867 
868  for(int i=(int)nbLvlPyr-1;i>=0;i--)
869  {
870  if(i>=(int)l0Pyr)
871  {
875  ptTemplateSupp=ptTemplateSuppPyr[i];
877  H=HdesirePyr[i];
878  HLM=HLMdesirePyr[i];
880  // zoneTracked=&zoneTrackedPyr[i];
881  trackRobust(pyr_I[i]);
882  }
883  //std::cout<<"get p up"<<std::endl;
884  // ptemp=p_sauv[i-1];
885  if (i > 0) {
886  Warp->getParamPyramidUp(p,ptemp);
887  p=ptemp;
889  }
890  }
891  #if 0
892  if(l0Pyr==0)
893  {
897  ptTemplateSupp=ptTemplateSuppPyr[0];
899  H=HdesirePyr[0];
900  HLM=HLMdesirePyr[0];
903  trackRobust(pyr_I[0]);
904  }
905 
906  if (l0Pyr > 0) {
907  // for (int l=(int)l0Pyr; l >=0; l--) {
908  // Warp->getParamPyramidUp(p,ptemp);
909  // p=ptemp;
910  // }
912  }
913  #endif
914  // delete [] p_sauv;
915  }
916  else
917  {
918  //std::cout<<"reviens a tracker de base"<<std::endl;
919  trackRobust(I);
920  }
921  delete[] pyr_I;
922  }
923  catch(vpException &e){
924  delete[] pyr_I;
926  }
927 }
928 
930 {
932  {
933  vpColVector p_pre_estimation;p_pre_estimation=p;
935  double pre_fcost=getCost(I,p);
936 
937  trackNoPyr(I);
938 
939  //std::cout<<"fct avant : "<<pre_fcost<<std::endl;
940  double post_fcost=getCost(I,p);
941  //std::cout<<"fct apres : "<<post_fcost<<std::endl<<std::endl;
942  if(pre_fcost<post_fcost)
943  p=p_pre_estimation;
944  }
945  else
946  trackNoPyr(I);
947 }
static void getGaussPyramidal(const vpImage< unsigned char > &I, vpImage< unsigned char > &GI)
vpDisplay * display
Definition: vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
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:226
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
void getGaussianBluredImage(const vpImage< unsigned char > &I)
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerPoint ** ptTemplatePyr
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
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:73
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:97
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:959
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:207
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:175
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:225
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)