Visual Servoing Platform  version 3.0.0
vpTemplateTrackerMIForwardAdditional.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/vpTemplateTrackerMIForwardAdditional.h>
41 
42 #ifdef VISP_HAVE_OPENMP
43 #include <omp.h>
44 #endif
45 
47  : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL),
48  threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()
49 {
50  evolRMS = 0;
51  x_pos = y_pos = NULL;
52  useCompositionnal=false;
53  threshold_RMS=1e-20;
54  minimizationMethod=USE_NEWTON;
55 }
56 
58 {
59  //std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
60 
61  dW=0;
62 
63  int Nbpoint=0;
64 
65  if(blur)
69 
70  double i2,j2;
71  double Tij;
72  double IW,dx,dy;
73  int cr,ct;
74  double er,et;
75 
76  int i,j;
77 
78  Nbpoint=0;
79 
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  X2[0]=j;X2[1]=i;
88 
89  Warp->computeDenom(X1,p);
90  Warp->warpX(X1,X2,p);
91 
92  j2=X2[0];i2=X2[1];
93 
94  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
95  {
96  Nbpoint++;
97  Tij=ptTemplate[point].val;
98  if(!blur)
99  IW=I.getValue(i2,j2);
100  else
101  IW=BI.getValue(i2,j2);
102 
103  dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
104  dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
105 
106  ct=(int)((IW*(Nc-1))/255.);
107  cr=(int)((Tij*(Nc-1))/255.);
108  et=(IW*(Nc-1))/255.-ct;
109  er=((double)Tij*(Nc-1))/255.-cr;
110  //std::cout<<"test"<<std::endl;
111  Warp->dWarp(X1,X2,p,dW);
112 
113  double *tptemp=new double[nbParam];
114  for(unsigned int it=0;it<nbParam;it++)
115  tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
116 
118  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
120  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
121 
122  delete[] tptemp;
123  }
124  }
125 
126  if(Nbpoint>0)
127  {
128  double MI;
129  computeProba(Nbpoint);
130  computeMI(MI);
132 
133 
134  // double conditionnement=GetConditionnement(Hdesire);
135  // std::cout<<"conditionnement : "<<conditionnement<<std::endl;
137  try
138  {
140  }
141  catch(vpException &e)
142  {
143  //std::cerr<<"probleme inversion"<<std::endl;
144  throw(e);
145  }
146  //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
147  //std::cout<<"\tEnd initialisation..."<<std::endl;
148  }
149 
150 }
151 
153 {
154  dW=0;
155 
156  double erreur=0;
157  int Nbpoint=0;
158  if(blur)
162 
163  double MI=0,MIprec=-1000;
164 
166 
167  double i2,j2;
168  double Tij;
169  double IW,dx,dy;
170  //unsigned
171  int cr,ct;
172  double er,et;
173  double alpha=2.;
174 
175  int i,j;
176  unsigned int iteration=0;
177 
178  initPosEvalRMS(p);
179  do
180  {
181  if(iteration%5==0)
183  Nbpoint=0;
184  MIprec=MI;
185  MI=0;
186  erreur=0;
187 
189 
190  Warp->computeCoeff(p);
191 #ifdef VISP_HAVE_OPENMP
192  int nthreads = omp_get_num_procs() ;
193  //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
194  omp_set_num_threads(nthreads);
195 #pragma omp parallel for private(Tij,IW,i,j,i2,j2,cr,ct,er,et,dx,dy) default(shared)
196 #endif
197  for(int point=0;point<(int)templateSize;point++)
198  {
199  i=ptTemplate[point].y;
200  j=ptTemplate[point].x;
201  X1[0]=j;X1[1]=i;
202 
203  Warp->computeDenom(X1,p);
204  Warp->warpX(X1,X2,p);
205 
206  j2=X2[0];i2=X2[1];
207 
208  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
209  {
210  Nbpoint++;
211  Tij=ptTemplate[point].val;
212  //Tij=Iterateurvecteur->val;
213  if(!blur)
214  IW=I.getValue(i2,j2);
215  else
216  IW=BI.getValue(i2,j2);
217 
218  dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
219  dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
220 
221  ct=(int)((IW*(Nc-1))/255.);
222  cr=(int)((Tij*(Nc-1))/255.);
223  et=(IW*(Nc-1))/255.-ct;
224  er=((double)Tij*(Nc-1))/255.-cr;
225 
226  //calcul de l'erreur
227  erreur+=(Tij-IW)*(Tij-IW);
228 
229  //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
230  Warp->dWarp(X1,X2,p,dW);
231 
232  //double *tptemp=temp;
233  double *tptemp=new double[nbParam];;
234  for(unsigned int it=0;it<nbParam;it++)
235  tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
236  //*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
237  //std::cout<<cr<<" "<<ct<<" ; ";
239  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
241  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
242 
243  delete[] tptemp;
244  }
245  }
246 
247  if(Nbpoint==0)
248  {
249  //std::cout<<"plus de point dans template suivi"<<std::endl;
250  diverge=true;
251  MI=0;
253  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
254  }
255  else
256  {
257  computeProba(Nbpoint);
258  computeMI(MI);
259  //std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
260  computeHessien(H);
261  computeGradient();
262 
264  try
265  {
266  switch(hessianComputation)
267  {
270  break;
272  if(HLM.cond()>HLMdesire.cond())
274  else
275  dp=gain*0.2*HLM.inverseByLU()*G;
276  break;
277  default:
278  dp=gain*0.2*HLM.inverseByLU()*G;
279  break;
280  }
281  }
282  catch(vpException &e)
283  {
284  //std::cerr<<"probleme inversion"<<std::endl;
286  throw(e);
287  }
288  }
289 
290  switch(minimizationMethod)
291  {
293  {
294  vpColVector p_test_LMA(nbParam);
296  p_test_LMA=p-100000.1*dp;
297  else
298  p_test_LMA=p+1.*dp;
299  MI=-getCost(I,p);
300  double MI_LMA=-getCost(I,p_test_LMA);
301  if(MI_LMA>MI)
302  {
303  p=p_test_LMA;
304  lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
305  }
306  else
307  {
308  lambda=(lambda*10.<1e6)?1e6:lambda*10.;
309  }
310  }
311  break;
313  {
314  dp=-gain*6.0*G;
315  if(useBrent)
316  {
317  alpha=2.;
318  computeOptimalBrentGain(I,p,-MI,dp,alpha);
319  dp=alpha*dp;
320  }
321  p+=1.*dp;
322  break;
323  }
324 
326  {
327  double s_scal_y;
328  if(iterationGlobale!=0)
329  {
330  vpColVector s_quasi=p-p_prec;
331  vpColVector y_quasi=G-G_prec;
332  s_scal_y=s_quasi.t()*y_quasi;
333  //if(s_scal_y!=0)//BFGS
334  // KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
335  //if(s_scal_y!=0)//DFP
336  if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
337  KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
338  }
339  dp=-KQuasiNewton*G;
340  p_prec=p;
341  G_prec=G;
342  p-=1.01*dp;
343  }
344  break;
345 
346  default:
347  {
349  dp=-0.1*dp;
350  if(useBrent)
351  {
352  alpha=2.;
353  computeOptimalBrentGain(I,p,-MI,dp,alpha);
354  //std::cout<<alpha<<std::endl;
355  dp=alpha*dp;
356  }
357 
358  p+=1.*dp;
359  break;
360  }
361  }
362 
363  computeEvalRMS(p);
364  iteration++;
366 
367  }
368  while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
369  //while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
370  if(Nbpoint==0) {
371  //std::cout<<"plus de point dans template suivi"<<std::endl;
373  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
374  }
375 
376  nbIteration=iteration;
379  {
380  MI_postEstimation = -1;
381  }
383 }
384 
386 {
387  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
388  x_pos=new double[nb_corners];
389  y_pos=new double[nb_corners];
390 
391  Warp->computeCoeff(pw);
392  vpTemplateTrackerTriangle triangle;
393 
394  for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
395  {
396  zoneTracked->getTriangle(i, triangle);
397  for (unsigned int j=0; j<3; j++) {
398  triangle.getCorner(j, X1[0], X1[1]);
399 
400  Warp->computeDenom(X1,pw);
401  Warp->warpX(X1,X2,pw);
402  x_pos[i*3+j]=X2[0];
403  y_pos[i*3+j]=X2[1];
404  }
405  }
406 }
407 
409 {
410  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
411 
412  Warp->computeCoeff(pw);
413  evolRMS=0;
414  vpTemplateTrackerTriangle triangle;
415 
416  for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
417  {
418  zoneTracked->getTriangle(i, triangle);
419  for (unsigned int j=0; j<3; j++) {
420  triangle.getCorner(j, X1[0], X1[1]);
421 
422  Warp->computeDenom(X1,pw);
423  Warp->warpX(X1,X2,pw);
424  evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
425  x_pos[i*3+j]=X2[0];
426  y_pos[i*3+j]=X2[1];
427  }
428  }
429  evolRMS=evolRMS/nb_corners;
430 }
431 
433 {
434  delete[] x_pos;
435  delete[] y_pos;
436 }
void computeHessien(vpMatrix &H)
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getWidth() const
Definition: vpImage.h:161
void trackNoPyr(const vpImage< unsigned char > &I)
vpTemplateTrackerPoint * ptTemplate
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
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
vpColVector getCorner(unsigned int i) const
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 > 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)
unsigned int getNbTriangle() const
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M)
vpImage< double > dIx
void computeMI(double &MI)
vpRowVector t() const
vpImage< double > dIy
unsigned int iterationGlobale
unsigned int nbIteration
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
vpTemplateTrackerZone * zoneTracked
unsigned int getHeight() const
Definition: vpImage.h:152
void initHessienDesired(const vpImage< unsigned char > &I)
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:3470
void computeProba(int &nbpoint)
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)=0
vpHessienType hessianComputation