Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpTemplateTrackerWarpHomographySL3.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 #include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
40 
41 //findWarp special a SL3 car methode additionnelle ne marche pas (la derivee n est calculable qu en p=0)
42 // => resout le probleme de maniere compositionnelle
43 void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0,const double *vt0,
44  const double *u, const double *v,int nb_pt,vpColVector& p)
45 {
46  //std::cout<<"findWarp OVERLOADE"<<std::endl;
47  vpColVector dp(nbParam);
48  vpMatrix dW_(2,nbParam);
49  vpMatrix dX(2,1);
51  vpMatrix G_(nbParam,1);
52 
53  //vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
54  double **dW_ddp0=new double*[(unsigned int)nb_pt];
55  for(int i=0;i<nb_pt;i++)
56  {
57  //dW_ddp0[i].resize(2,nbParam);
58  dW_ddp0[i]=new double[2*nbParam];
59  //getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
60  //std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
61  getdWdp0(v[i],u[i],dW_ddp0[i]);
62  }
63 
64  int cpt=0;
65  vpColVector X1(2);
66  vpColVector fX1(2);
67  vpColVector X2(2);
68  double erreur=0;
69  double erreur_prec;
70  double lambda=0.00001;
71  do
72  {
73  erreur_prec=erreur;
74  H=0;
75  G_=0;
76  erreur=0;
77  computeCoeff(p);
78  for(int i=0;i<nb_pt;i++)
79  {
80  X1[0]=ut0[i];
81  X1[1]=vt0[i];
82  computeDenom(X1,p);
83  warpX(X1,fX1,p);
84  //dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
85  //dWarp(X1,fX1,p,dW);
86  for(unsigned int ip=0;ip<nbParam;ip++)
87  {
88  dW_[0][ip]=dW_ddp0[i][ip];
89  dW_[1][ip]=dW_ddp0[i][ip+nbParam];
90  }
91 
92  H+=dW_.AtA();
93 
94  X2[0]=u[i];
95  X2[1]=v[i];
96 
97  dX=X2-fX1;
98  G_+=dW_.t()*dX;
99 
100  erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
101  }
102 
103  vpMatrix::computeHLM(H, lambda, HLM);
104  try{
105  dp=HLM.inverseByLU()*G_;
106  }
107  catch(vpException &e){
108  //std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
109  throw(e);
110  }
111  pRondp(p,dp,p);
112 
113  cpt++;
114  // std::cout<<"erreur ="<<erreur<<std::endl;
115  }
116  //while((cpt<1500));
117  while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
118 
119  //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
120  for(int i=0;i<nb_pt;i++)
121  delete[] dW_ddp0[i];
122  delete[] dW_ddp0;
123 
124 }
125 
127  : G(), dGx(), A()
128 {
129  nbParam = 8 ;
130  G.resize(3,3);
131  dGx.resize(3,nbParam);
132 
133  A.resize(8);
134  for(unsigned int i=0;i<8;i++)
135  {
136  A[i].resize(3,3);
137  A[i]=0;
138  }
139  A[0][0][2]=1;
140  A[1][1][2]=1;
141  A[2][0][1]=1;
142  A[3][1][0]=1;
143  A[4][0][0]=1;
144  A[4][1][1]=-1;
145  A[5][1][1]=-1;
146  A[5][2][2]=1;
147  A[6][2][0]=1;
148  A[7][2][1]=1;
149 }
150 
152 {
153 }
154 
155 //get the parameter corresponding to the lower level of a gaussian pyramid
156 //a refaire de facon analytique
158 {
159  double *u,*v;u=new double[4];v=new double[4];
160  //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
161  u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
162  double *u2,*v2;u2=new double[4];v2=new double[4];
163  warp(u,v,4,p,u2,v2);
164  //p=0;findWarp(u,v,u2,v2,4,p);
165  for(int i=0;i<4;i++)
166  {
167  u[i]=u[i]/2.;
168  v[i]=v[i]/2.;
169  u2[i]=u2[i]/2.;
170  v2[i]=v2[i]/2.;
171  //std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
172  }
173  pdown=p;
174  findWarp(u,v,u2,v2,4,pdown);
175  delete[] u;
176  delete[] v;
177  delete[] u2;
178  delete[] v2;
179 }
180 
182 {
183  double *u,*v;u=new double[4];v=new double[4];
184  //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
185  u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
186  //u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
187  double *u2,*v2;u2=new double[4];v2=new double[4];
188 
189  //pup=p;
190 
191  /*vpColVector ptest=pup;
192  warp(u,v,4,ptest,u2,v2);
193  for(int i=0;i<4;i++)
194  std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
195 
196  warp(u,v,4,p,u2,v2);
197  //p=0;findWarp(u,v,u2,v2,4,p);
198 
199 
200  for(int i=0;i<4;i++)
201  {
202  u[i]=u[i]*2.;
203  v[i]=v[i]*2.;
204  u2[i]=u2[i]*2.;
205  v2[i]=v2[i]*2.;
206  /*std::cout<<"#########################################################################################"<<std::endl;
207  std::cout<<"#########################################################################################"<<std::endl;
208  std::cout<<"#########################################################################################"<<std::endl;
209  std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
210  }
211  findWarp(u,v,u2,v2,4,pup);
212 
213  delete[] u;
214  delete[] v;
215  delete[] u2;
216  delete[] v2;
217 }
218 
220 {
221  denom=vX[0]*G[2][0]+vX[1]*G[2][1]+G[2][2];
222 }
223 
225 {
226  vpMatrix pA(3,3);
227  pA[0][0]=p[4];
228  pA[0][1]=p[2];
229  pA[0][2]=p[0];
230 
231  pA[1][0]=p[3];
232  pA[1][1]=-p[4]-p[5];
233  pA[1][2]=p[1];
234 
235  pA[2][0]=p[6];
236  pA[2][1]=p[7];
237  pA[2][2]=p[5];
238 
239  G=pA.expm();
240 }
241 
242 
244 {
245  double i=vX[1],j=vX[0];
246  vXres[0]=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
247  vXres[1]=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
248 }
249 void vpTemplateTrackerWarpHomographySL3::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &/*ParamM*/)
250 {
251  j2=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
252  i2=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
253 }
254 
256 {
257  vpHomography H;
258  for (unsigned int i=0; i<3; i++)
259  for (unsigned int j=0; j<3; j++)
260  H[i][j] = G[i][j];
261  return H;
262 }
263 
265 {
266  vpMatrix dhdx(2,3);
267  dhdx=0;
268  dhdx[0][0]=1./denom;dhdx[1][1]=1./denom;dhdx[0][2]=-X2[0]/(denom);dhdx[1][2]=-X2[1]/(denom);
269  dGx=0;
270  for(unsigned int i=0;i<3;i++)
271  {
272  dGx[i][0]=G[i][0];
273  dGx[i][1]=G[i][1];
274  dGx[i][2]=G[i][0]*X1[1];
275  dGx[i][3]=G[i][1]*X1[0];
276  dGx[i][4]=G[i][0]*X1[0]-G[i][1]*X1[1];
277  dGx[i][5]=G[i][2]-G[i][1]*X1[1];
278  dGx[i][6]=G[i][2]*X1[0];
279  dGx[i][7]=G[i][2]*X1[1];
280  }
281  dW_=dhdx*dGx;
282 
283 }
284 
285 /*calcul de di*dw(x,p0)/dp
286 */
287 void vpTemplateTrackerWarpHomographySL3::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
288 {
289  vpMatrix dhdx(1,3);
290  dhdx=0;
291  dhdx[0][0]=dx;dhdx[0][1]=dy;dhdx[0][2]=-j*dx-i*dy;
292  G.eye();
293 
294  dGx=0;
295  for(unsigned int par=0;par<3;par++)
296  {
297  dGx[par][0]=G[par][0];
298  dGx[par][1]=G[par][1];
299  dGx[par][2]=G[par][0]*i;
300  dGx[par][3]=G[par][1]*j;
301  dGx[par][4]=G[par][0]*j-G[par][1]*i;
302  dGx[par][5]=G[par][2]-G[par][1]*i;
303  dGx[par][6]=G[par][2]*j;
304  dGx[par][7]=G[par][2]*i;
305  }
306 
307  for(unsigned int par=0;par<nbParam;par++)
308  {
309  double res=0;
310  for(unsigned int par2=0;par2<3;par2++)
311  res+=dhdx[0][par2]*dGx[par2][par];
312  dIdW[par]=res;
313  }
314 
315 }
316 /*calcul de dw(x,p0)/dp
317 */
318 
319 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &i,const int &j,double *dIdW)
320 {
321  vpMatrix dhdx(2,3);
322  dhdx=0;
323  dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
324  G.eye();
325 
326  dGx=0;
327  for(unsigned int par=0;par<3;par++)
328  {
329  dGx[par][0]=G[par][0];
330  dGx[par][1]=G[par][1];
331  dGx[par][2]=G[par][0]*i;
332  dGx[par][3]=G[par][1]*j;
333  dGx[par][4]=G[par][0]*j-G[par][1]*i;
334  dGx[par][5]=G[par][2]-G[par][1]*i;
335  dGx[par][6]=G[par][2]*j;
336  dGx[par][7]=G[par][2]*i;
337  }
338  vpMatrix dIdW_temp(2,nbParam);
339  dIdW_temp=dhdx*dGx;
340 
341  for(unsigned int par=0;par<nbParam;par++)
342  {
343  dIdW[par]=dIdW_temp[0][par];
344  dIdW[par+nbParam]=dIdW_temp[1][par];
345  }
346 
347 }
348 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &i,const double &j,double *dIdW)
349 {
350  vpMatrix dhdx(2,3);
351  dhdx=0;
352  dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
353  G.eye();
354 
355  dGx=0;
356  for(unsigned int par=0;par<3;par++)
357  {
358  dGx[par][0]=G[par][0];
359  dGx[par][1]=G[par][1];
360  dGx[par][2]=G[par][0]*i;
361  dGx[par][3]=G[par][1]*j;
362  dGx[par][4]=G[par][0]*j-G[par][1]*i;
363  dGx[par][5]=G[par][2]-G[par][1]*i;
364  dGx[par][6]=G[par][2]*j;
365  dGx[par][7]=G[par][2]*i;
366  }
367  vpMatrix dIdW_temp(2,nbParam);
368  dIdW_temp=dhdx*dGx;
369 
370  for(unsigned int par=0;par<nbParam;par++)
371  {
372  dIdW[par]=dIdW_temp[0][par];
373  dIdW[par+nbParam]=dIdW_temp[1][par];
374  }
375 
376 }
377 /*compute dw=dw/dx*dw/dp
378 */
379 
381  const double *dwdp0,vpMatrix &dW_)
382 {
383  for(unsigned int i=0;i<nbParam;i++)
384  {
385  dW_[0][i]=denom*((G[0][0]-X2[0]*G[2][0])*dwdp0[i]+(G[0][1]-X2[0]*G[2][1])*dwdp0[i+nbParam]);
386  dW_[1][i]=denom*((G[1][0]-X2[1]*G[2][0])*dwdp0[i]+(G[1][1]-X2[1]*G[2][1])*dwdp0[i+nbParam]);
387  }
388 }
389 
391 {
392  ParamMinv=-ParamM;
393 }
395 {
396  //vrai que si commutatif ...
397  pres=p1+p2;
398 }
399 
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void computeDenom(vpColVector &vX, const vpColVector &ParamM)
void getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
vpMatrix expm() const
Definition: vpMatrix.cpp:3372
void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)
void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW)
error that can be emited by ViSP classes.
Definition: vpException.h:73
void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p)
void getdWdp0(const int &i, const int &j, double *dIdW)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
vpMatrix AtA() const
Definition: vpMatrix.cpp:376
void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
void getParamPyramidUp(const vpColVector &p, vpColVector &pup)
vpMatrix t() const
Definition: vpMatrix.cpp:207
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:3525
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)
void eye()
Definition: vpMatrix.cpp:194