Visual Servoing Platform  version 3.0.0
vpTemplateTrackerMIESM.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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  * Example of template tracking.
32  *
33  * Authors:
34  * Amaury Dame
35  * Aurelien Yol
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <visp3/tt_mi/vpTemplateTrackerMIESM.h>
41 
42 #ifdef VISP_HAVE_OPENMP
43 #include <omp.h>
44 #endif
45 
47  : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), CompoInitialised(false),
48  HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
49 {
50  useCompositionnal=false;
51  useInverse=false;
52  if(!Warp->isESMcompatible())
53  std::cerr<<"The selected warp function is not appropriate for the ESM algorithm..."<<std::endl;
54 }
55 
57 {
59  std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
60 
61  dW=0;
62 
63  double erreur=0;
64  int Nbpoint=0;
65 
66  double i2,j2;
67  double Tij;
68  double IW,dx,dy;
69  //int cr,ct;
70  //double er,et;
71  int i,j;
72 
73  Nbpoint=0;
74  erreur=0;
75 
76  if(blur)
78 
80 
81  Warp->computeCoeff(p);
82  for(unsigned int point=0;point<templateSize;point++)
83  {
84  i=ptTemplate[point].y;
85  j=ptTemplate[point].x;
86  X1[0]=j;X1[1]=i;
87 
88  Warp->computeDenom(X1,p);
89  Warp->warpX(X1,X2,p);
90 
91  j2=X2[0];i2=X2[1];
92 
93  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
94  {
95  Nbpoint++;
96 // if(blur)
97 // IW=BI.getValue(i2,j2);
98 // else
99 // IW=I.getValue(i2,j2);
100 
101  //ct=ptTemplateSupp[point].ct;
102  //et=ptTemplateSupp[point].et;
103  //cr=(int)((IW*(Nc-1))/255.);
104  //er=((double)IW*(Nc-1))/255.-cr;
105 
106  // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
107  // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
108  // else
109  // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
110 
111  // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,false);
112  }
113  }
114 
115 
116  double MI;
117  computeProba(Nbpoint);
118  computeMI(MI);
120  //std::cout<<"HdesireInverse"<<std::endl<<HdesireInverse<<std::endl;
121 
123  // DIRECT COMPO
124 
128  {
132  }
133 
134  Nbpoint=0;
135  erreur=0;
137 
138  Warp->computeCoeff(p);
139  for(unsigned int point=0;point<templateSize;point++)
140  {
141  i=ptTemplate[point].y;
142  j=ptTemplate[point].x;
143  X1[0]=j;X1[1]=i;
144 
145  Warp->computeDenom(X1,p);
146  Warp->warpX(X1,X2,p);
147 
148  j2=X2[0];i2=X2[1];
149 
150  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight())&&(j2<I.getWidth()))
151  {
152  Nbpoint++;
153  Tij=ptTemplate[point].val;
154  if(!blur)
155  IW=I.getValue(i2,j2);
156  else
157  IW=BI.getValue(i2,j2);
158 
159  dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
160  dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
161 
162  //cr=ptTemplateSupp[point].ct;
163  //er=ptTemplateSupp[point].et;
164  //ct=(int)((IW*(Nc-1))/255.);
165  //et=((double)IW*(Nc-1))/255.-ct;
166 
167  Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
168 
169  double *tptemp=new double[nbParam];
170  for(unsigned int it=0;it<nbParam;it++)
171  tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
172 
173 
174  // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,
175  // hessianComputation== vpTemplateTrackerMI::USE_HESSIEN_DESIRE);
176  //calcul de l'erreur
177  erreur+=(Tij-IW)*(Tij-IW);
178 
179  // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
180  // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);
181  // else
182  // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);
183 
184  // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,false);
185 
186  delete[] tptemp;
187  }
188  }
189 
190  computeProba(Nbpoint);
191  computeMI(MI);
193  //std::cout<<"HdesireDirect"<<std::endl<<HdesireDirect<<std::endl;
194 
196 
198  //Hdesire=HdesireDirect;
199  //Hdesire=HdesireInverse;
200 
201  //std::cout<<"HdesireDirect "<<HdesireDirect<<std::endl;
202  //std::cout<<"HdesireInverse "<<HdesireInverse<<std::endl;
205 
206  //std::cout<<"\tEnd initialisation..."<<std::endl;
207 
208 }
209 
211 {
212  //std::cout<<"Initialise precomputed value of ESM Tracker"<<std::endl;
213 
220 
221  ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
223  int i,j;
224  double et;
225  int ct;
226  double Tij;
227  for(unsigned int point=0;point<templateSize;point++)
228  {
229  i=ptTemplate[point].y;
230  j=ptTemplate[point].x;
231  X1[0]=j;X1[1]=i;
232  Warp->computeDenom(X1,p);
233 
234  ptTemplateCompo[point].dW=new double[2*nbParam];
235  Warp->getdWdp0(i,j,ptTemplateCompo[point].dW);
236 
237  ptTemplate[point].dW=new double[nbParam];
238  double dx=ptTemplate[point].dx*(Nc-1)/255.;
239  double dy=ptTemplate[point].dy*(Nc-1)/255.;
240  Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
241 
242  Tij=ptTemplate[point].val;
243  ct=(int)((Tij*(Nc-1))/255.);
244  et=(Tij*(Nc-1))/255.-ct;
245  ptTemplateSupp[point].et=et;
246  ptTemplateSupp[point].ct=ct;
247  ptTemplateSupp[point].Bt=new double[4];
248  ptTemplateSupp[point].dBt=new double[4];
249  for(char it=-1;it<=2;it++)
250  {
251  ptTemplateSupp[point].Bt[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
252  ptTemplateSupp[point].dBt[it+1] =vpTemplateTrackerMIBSpline::dBspline4(-it+et);
253  }
254  }
255  CompoInitialised=true;
256 }
257 
259 {
260  if(!CompoInitialised)
261  std::cout<<"Compositionnal tracking no initialised\nUse initCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
262  dW=0;
263 
264  if(blur)
268  /* if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
269  {
270  getGradX(dIx, d2Ix,fgdG,taillef);
271  getGradY(dIx, d2Ixy,fgdG,taillef);
272  getGradY(dIy, d2Iy,fgdG,taillef);
273  }*/
274 
275  double erreur=0;
276  int Nbpoint=0;
277  int point;
278 
280 
282  double MI=0;
283  //double MIprec=-1000;
284 
285  double i2,j2;
286  double Tij;
287  double IW;
288  //int cr,ct;
289  //double er,et;
290 
291  vpColVector dpinv(nbParam);
292 
293  double dx,dy;
294  double alpha=2.;
295 
296  int i,j;
297  unsigned int iteration=0;
298  do
299  {
300  Nbpoint=0;
301  //MIprec=MI;
302  MI=0;
303  erreur=0;
304 
306 
308  // Inverse
309  Warp->computeCoeff(p);
310  for(point=0;point<(int)templateSize;point++)
311  {
312  i=ptTemplate[point].y;
313  j=ptTemplate[point].x;
314  X1[0]=j;X1[1]=i;
315 
316  Warp->computeDenom(X1,p);
317  Warp->warpX(X1,X2,p);
318 
319  j2=X2[0];i2=X2[1];
320 
321  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
322  {
323  Nbpoint++;
324  //Tij=ptTemplate[point].val;
325  //if(!blur)
326  // IW=I.getValue(i2,j2);
327  //else
328  // IW=BI.getValue(i2,j2);
329 
330  //ct=ptTemplateSupp[point].ct;
331  //et=ptTemplateSupp[point].et;
332  //cr=(int)((IW*(Nc-1))/255.);
333  //er=((double)IW*(Nc-1))/255.-cr;
334 
335  // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) // cas à tester AY
336  // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
337  // else
338  // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
339  }
340  }
341 
342  if(Nbpoint==0)
343  {
344  //std::cout<<"plus de point dans template suivi"<<std::endl;
345  diverge=true;
346  MI=0;
347  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
348  }
349  else
350  {
351  computeProba(Nbpoint);
352  computeMI(MI);
355  computeGradient();
356  GInverse=G;
357 
359  // DIRECT
360 
361  Nbpoint=0;
362  //MIprec=MI;
363  MI=0;
364  erreur=0;
365 
367 
368  Warp->computeCoeff(p);
369 #ifdef VISP_HAVE_OPENMP
370  int nthreads = omp_get_num_procs() ;
371  //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
372  omp_set_num_threads(nthreads);
373 #pragma omp parallel for private(point, Tij,IW,i,j,i2,j2,/*cr,ct,er,et,*/dx,dy) default(shared)
374 #endif
375  for(point=0;point<(int)templateSize;point++)
376  {
377  i=ptTemplate[point].y;
378  j=ptTemplate[point].x;
379  X1[0]=j;X1[1]=i;
380  Warp->computeDenom(X1,p);
381  Warp->warpX(i,j,i2,j2,p);
382  X2[0]=j2;X2[1]=i2;
383 
384  //Warp->computeDenom(X1,p);
385  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
386  {
387  Nbpoint++;
388  Tij=ptTemplate[point].val;
389  //Tij=Iterateurvecteur->val;
390  if(!blur)
391  IW=I.getValue(i2,j2);
392  else
393  IW=BI.getValue(i2,j2);
394 
395  dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
396  dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
397 
398  //ct=(int)((IW*(Nc-1))/255.);
399  //et=((double)IW*(Nc-1))/255.-ct;
400  //cr=ptTemplateSupp[point].ct;
401  //er=ptTemplateSupp[point].et;
402 
403  Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
404 
405  double *tptemp=new double[nbParam];
406  for(unsigned int it=0;it<nbParam;it++)
407  tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
408 
409 
410  //calcul de l'erreur
411  erreur+=(Tij-IW)*(Tij-IW);
412  // if(bspline==3)
413  // vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
414  // else
415  // vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
416  // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
417  // hessianComputation==vpHessienType::USE_HESSIEN_DESIRE);
418 
419  delete[] tptemp;
420 
421  }
422  }
423 
424  computeProba(Nbpoint);
425  computeMI(MI);
428  computeGradient();
429  GDirect=G;
430 
432  {
435  }
437  //G=GDirect;
438  //G=-GInverse;
439 
440  try
441  {
442  if(minimizationMethod==vpTemplateTrackerMIESM::USE_GRADIENT)
443  dp=-gain*0.3*G;
444  else
445  {
446  switch(hessianComputation)
447  {
450  break;
452  if(HLM.cond()>HLMdesire.cond())
454  else
455  dp=gain*0.3*HLM.inverseByLU()*G;
456  break;
457  default:
458  dp=gain*0.3*HLM.inverseByLU()*G;
459  break;
460  }
461  }
462  }
463  catch(vpException &e)
464  {
465  //std::cerr<<"probleme inversion"<<std::endl;
466  throw(e);
467  }
468 
470  dp=-1.*dp;
471 
472  if(useBrent)
473  {
474  alpha=2.;
475  computeOptimalBrentGain(I,p,-MI,dp,alpha);
476  dp=alpha*dp;
477  }
478  p+=dp;
479 
480  iteration++;
481  }
482  }
483  while( /*(MI!=MIprec) &&*/(iteration< iterationMax));
484 
487  {
488  MI_postEstimation = -1;
489  }
490 
491  nbIteration=iteration;
492 }
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW)=0
void computeHessien(vpMatrix &H)
unsigned int getWidth() const
Definition: vpImage.h:161
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
vpTemplateTrackerPoint * ptTemplate
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
vpTemplateTrackerMIESM()
Default constructor.
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
Type getValue(double i, double j) const
Definition: vpImage.h:1148
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
vpImage< double > d2Ix
vpImage< double > BI
vpHessienApproximationType ApproxHessian
double cond() const
Definition: vpMatrix.cpp:3445
unsigned int templateSize
unsigned int iterationMax
Error that can be emited by the vpTracker class and its derivates.
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
void initHessienDesired(const vpImage< unsigned char > &I)
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M)
vpImage< double > dIx
void computeMI(double &MI)
vpImage< double > dIy
vpImage< double > d2Iy
virtual bool isESMcompatible() const =0
virtual void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)=0
unsigned int nbIteration
void trackNoPyr(const vpImage< unsigned char > &I)
vpMinimizationTypeMIESM minimizationMethod
vpTemplateTrackerPointCompo * ptTemplateCompo
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpImage< double > d2Ixy
vpMatrix inverseByLU() const
unsigned int getHeight() const
Definition: vpImage.h:152
virtual void getdWdp0(const int &i, const int &j, double *dIdW)=0
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:3470
void computeProba(int &nbpoint)
vpHessienType hessianComputation
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217