ViSP  2.10.0
vpTemplateTrackerZNCCForwardAdditional.cpp
1 /****************************************************************************
2  *
3  * $Id: vpTemplateTrackerZNCCForwardAdditional.cpp 4783 2014-07-15 13:07:52Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Template tracker.
35  *
36  * Authors:
37  * Amaury Dame
38  * Aurelien Yol
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 #include <visp/vpTemplateTrackerZNCCForwardAdditional.h>
43 #include <visp/vpImageFilter.h>
44 
46 {
47  useCompositionnal=false;
48 }
49 
51 {
52  if(blur)
56 
57  vpImage<double> dIxx,dIxy,dIyx,dIyy;
60 
63 
64  Warp->computeCoeff(p);
65  double IW,dIWx,dIWy;
66  double Tij;
67  int i,j;
68  double i2,j2;
69  int Nbpoint=0;
70 
71  double moyTij=0;
72  double moyIW=0;
73  double denom=0;
74  for(unsigned int point=0;point<templateSize;point++)
75  {
76  i=ptTemplate[point].y;
77  j=ptTemplate[point].x;
78  X1[0]=j;X1[1]=i;
79  X2[0]=j;X2[1]=i;
80 
81  Warp->computeDenom(X1,p);
82 
83  j2=X2[0];i2=X2[1];
84 
85  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
86  {
87  Tij=ptTemplate[point].val;
88 
89  if(!blur)
90  IW=I.getValue(i2,j2);
91  else
92  IW=BI.getValue(i2,j2);
93 
94  Nbpoint++;
95  moyTij+=Tij;
96  moyIW+=IW;
97  }
98  }
99  moyTij=moyTij/Nbpoint;
100  moyIW=moyIW/Nbpoint;
101  Hdesire=0;
102  for(unsigned int point=0;point<templateSize;point++)
103  {
104  i=ptTemplate[point].y;
105  j=ptTemplate[point].x;
106  X1[0]=j;X1[1]=i;
107  X2[0]=j;X2[1]=i;
108 
109  Warp->computeDenom(X1,p);
110 
111  j2=X2[0];i2=X2[1];
112 
113  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
114  {
115  Tij=ptTemplate[point].val;
116 
117  if(!blur)
118  IW=I.getValue(i2,j2);
119  else
120  IW=BI.getValue(i2,j2);
121 
122  dIWx=dIx.getValue(i2,j2);
123  dIWy=dIy.getValue(i2,j2);
124  //Calcul du Hessien
125  Warp->dWarp(X1,X2,p,dW);
126  double *tempt=new double[nbParam];
127  for(unsigned int it=0;it<nbParam;it++)
128  tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
129 
130 
131  double prod=(Tij-moyTij);
132 
133  double d_Ixx=dIxx.getValue(i2,j2);
134  double d_Iyy=dIyy.getValue(i2,j2);
135  double d_Ixy=dIxy.getValue(i2,j2);
136 
137  for(unsigned int it=0;it<nbParam;it++)
138  for(unsigned int jt=0;jt<nbParam;jt++)
139  Hdesire[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
140  +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
141  /*Hdesire[0][0]+=prod*d_Ixx;
142  Hdesire[1][0]+=prod*d_Ixy;
143  Hdesire[0][1]+=prod*d_Ixy;
144  Hdesire[1][1]+=prod*d_Iyy;*/
145 
146  denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
147  delete[] tempt;
148  }
149 
150 
151  }
152 
153  Hdesire=Hdesire/sqrt(denom);
156  //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
157 
158 }
159 
161 {
162  double erreur=0;
163  int Nbpoint=0;
164 
165  if(blur)
169 
170  /*vpImage<double> dIxx,dIxy,dIyx,dIyy;
171  getGradX(dIx, dIxx, fgdG,taillef);
172  getGradY(dIx, dIxy, fgdG,taillef);
173 
174  getGradX(dIy, dIyx, fgdG,taillef);
175  getGradY(dIy, dIyy, fgdG,taillef);*/
176 
177  dW=0;
178 
179  //double lambda=lambdaDep;
180  double IW,dIWx,dIWy;
181  double Tij;
182  unsigned int iteration=0;
183  int i,j;
184  double i2,j2;
185  double alpha=2.;
186  do
187  {
188  Nbpoint=0;
189  erreur=0;
190  G=0;
191  H=0 ;
192  Warp->computeCoeff(p);
193  double moyTij=0;
194  double moyIW=0;
195  double denom=0;
196  for(unsigned int point=0;point<templateSize;point++)
197  {
198  i=ptTemplate[point].y;
199  j=ptTemplate[point].x;
200  X1[0]=j;X1[1]=i;
201 
202  Warp->computeDenom(X1,p);
203  Warp->warpX(X1,X2,p);
204 
205  j2=X2[0];i2=X2[1];
206  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
207  {
208  Tij=ptTemplate[point].val;
209 
210  if(!blur)
211  IW=I.getValue(i2,j2);
212  else
213  IW=BI.getValue(i2,j2);
214 
215  Nbpoint++;
216  moyTij+=Tij;
217  moyIW+=IW;
218  }
219  }
220 
221  if(! Nbpoint) {
223  "Cannot track the template: no point")) ;
224  }
225 
226  moyTij=moyTij/Nbpoint;
227  moyIW=moyIW/Nbpoint;
228  //vpMatrix d2Wx(nbParam,nbParam);
229  //vpMatrix d2Wy(nbParam,nbParam);
230  for(unsigned int point=0;point<templateSize;point++)
231  {
232  i=ptTemplate[point].y;
233  j=ptTemplate[point].x;
234  X1[0]=j;X1[1]=i;
235 
236  Warp->computeDenom(X1,p);
237  Warp->warpX(X1,X2,p);
238 
239  j2=X2[0];i2=X2[1];
240  if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
241  {
242  Tij=ptTemplate[point].val;
243 
244  if(!blur)
245  IW=I.getValue(i2,j2);
246  else
247  IW=BI.getValue(i2,j2);
248 
249  dIWx=dIx.getValue(i2,j2);
250  dIWy=dIy.getValue(i2,j2);
251  //Calcul du Hessien
252  Warp->dWarp(X1,X2,p,dW);
253  double *tempt=new double[nbParam];
254  for(unsigned int it=0;it<nbParam;it++)
255  tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
256 
257 
258  double prod=(Tij-moyTij);
259  for(unsigned int it=0;it<nbParam;it++)
260  G[it]+=prod*tempt[it];
261 
262  /* Warp->d2Warp(X1,X2,p,d2Wx,d2Wy);
263  for(int it=0;it<nbParam;it++)
264  for(int jt=0;jt<nbParam;jt++)
265  H[it][jt]+=prod*(d2Wx[it][jt]*dIWx+d2Wx[it][jt]*dIWy);*/
266  /*double d_Ixx=dIxx.getValue(i2,j2);
267  double d_Iyy=dIyy.getValue(i2,j2);
268  double d_Ixy=dIxy.getValue(i2,j2);
269 
270  for(int it=0;it<nbParam;it++)
271  for(int jt=0;jt<nbParam;jt++)
272  H[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
273  +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));*/
274  /*H[0][0]+=prod*d_Ixx;
275  H[1][0]+=prod*d_Ixy;
276  H[0][1]+=prod*d_Ixy;
277  H[1][1]+=prod*d_Iyy;*/
278 
279  double er=(Tij-IW);
280  erreur+=(er*er);
281  denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
282  delete[] tempt;
283  }
284 
285 
286  }
287  /*std::cout<<"G="<<G<<std::endl;
288  std::cout<<"H="<<H<<std::endl;
289  std::cout<<" denom="<<denom<<std::endl;*/
290  G=G/sqrt(denom);
291  //std::cout<<G<<std::endl;
292  H=H/sqrt(denom);
293 
294  //if(Nbpoint==0)std::cout<<"plus de point dans template suivi"<<std::endl; // cannot occur
295 
296  try
297  {
298  //vpMatrix::computeHLM(H,lambda,HLM);
299  //dp=1.*HLM.inverseByLU()*G;
300  dp=1.*HLMdesireInverse*G;
301  }
302  catch(vpException &e)
303  {
304  //std::cout<<"probleme inversion"<<std::endl;
305  throw(e);
306  }
307 
308  dp=gain*dp;
309  if(useBrent)
310  {
311  alpha=2.;
312  computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
313  dp=alpha*dp;
314  }
315  p-=dp;
316  iteration++;
317  }
318  while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));
319 
320  //std::cout<<"erreur "<<erreur<<std::endl;
321  nbIteration=iteration;
322 }
323 
unsigned int getWidth() const
Definition: vpImage.h:161
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
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:76
Type getValue(double i, double j) const
Definition: vpImage.h:1068
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
unsigned int templateSize
unsigned int iterationMax
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M)
vpImage< double > dIx
vpImage< double > dIy
unsigned int nbIteration
void initHessienDesired(const vpImage< unsigned char > &I)
vpMatrix inverseByLU() const
unsigned int getHeight() const
Definition: vpImage.h:152
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:4320
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)=0