Visual Servoing Platform  version 3.2.1 under development (2019-08-08)
vpTemplateTrackerWarpHomographySL3.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Template tracker.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 #include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
41 
42 // findWarp special a SL3 car methode additionnelle ne marche pas (la derivee
43 // n est calculable qu en p=0)
44 // => resout le probleme de maniere compositionnelle
45 void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0, const double *vt0, const double *u,
46  const double *v, int nb_pt, vpColVector &p)
47 {
48  // std::cout<<"findWarp OVERLOADE"<<std::endl;
49  vpColVector dp(nbParam);
50  vpMatrix dW_(2, nbParam);
51  vpMatrix dX(2, 1);
53  vpMatrix G_(nbParam, 1);
54 
55  // vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
56  double **dW_ddp0 = new double *[(unsigned int)nb_pt];
57  for (int i = 0; i < nb_pt; i++) {
58  // dW_ddp0[i].resize(2,nbParam);
59  dW_ddp0[i] = new double[2 * nbParam];
60  // getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
61  // std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
62  getdWdp0(v[i], u[i], dW_ddp0[i]);
63  }
64 
65  int cpt = 0;
66  vpColVector X1(2);
67  vpColVector fX1(2);
68  vpColVector X2(2);
69  double erreur = 0;
70  double erreur_prec;
71  double lambda = 0.00001;
72  do {
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  X1[0] = ut0[i];
80  X1[1] = vt0[i];
81  computeDenom(X1, p);
82  warpX(X1, fX1, p);
83  // dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
84  // dWarp(X1,fX1,p,dW);
85  for (unsigned int ip = 0; ip < nbParam; ip++) {
86  dW_[0][ip] = dW_ddp0[i][ip];
87  dW_[1][ip] = dW_ddp0[i][ip + nbParam];
88  }
89 
90  H += dW_.AtA();
91 
92  X2[0] = u[i];
93  X2[1] = v[i];
94 
95  dX = X2 - fX1;
96  G_ += dW_.t() * dX;
97 
98  erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + (v[i] - fX1[1]) * (v[i] - fX1[1]));
99  }
100 
101  vpMatrix::computeHLM(H, lambda, HLM);
102  try {
103  dp = HLM.inverseByLU() * G_;
104  } catch (const vpException &e) {
105  // std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
106  throw(e);
107  }
108  pRondp(p, dp, p);
109 
110  cpt++;
111  // std::cout<<"erreur ="<<erreur<<std::endl;
112  }
113  // while((cpt<1500));
114  while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
115 
116  // std::cout<<"erreur apres transformation="<<erreur<<std::endl;
117  for (int i = 0; i < nb_pt; i++)
118  delete[] dW_ddp0[i];
119  delete[] dW_ddp0;
120 }
121 
123 {
124  nbParam = 8;
125  G.resize(3, 3);
126  dGx.resize(3, nbParam);
127 
128  A.resize(8);
129  for (unsigned int i = 0; i < 8; i++) {
130  A[i].resize(3, 3);
131  A[i] = 0;
132  }
133  A[0][0][2] = 1;
134  A[1][1][2] = 1;
135  A[2][0][1] = 1;
136  A[3][1][0] = 1;
137  A[4][0][0] = 1;
138  A[4][1][1] = -1;
139  A[5][1][1] = -1;
140  A[5][2][2] = 1;
141  A[6][2][0] = 1;
142  A[7][2][1] = 1;
143 }
144 
146 
147 // get the parameter corresponding to the lower level of a gaussian pyramid
148 // a refaire de facon analytique
150 {
151  double *u, *v;
152  u = new double[4];
153  v = new double[4];
154  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
155  u[0] = 0;
156  v[0] = 0;
157  u[1] = 160;
158  v[1] = 0;
159  u[2] = 160;
160  v[2] = 120;
161  u[3] = 0;
162  v[3] = 120;
163  double *u2, *v2;
164  u2 = new double[4];
165  v2 = new double[4];
166  warp(u, v, 4, p, u2, v2);
167  // p=0;findWarp(u,v,u2,v2,4,p);
168  for (int i = 0; i < 4; i++) {
169  u[i] = u[i] / 2.;
170  v[i] = v[i] / 2.;
171  u2[i] = u2[i] / 2.;
172  v2[i] = v2[i] / 2.;
173  // std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
174  }
175  pdown = p;
176  findWarp(u, v, u2, v2, 4, pdown);
177  delete[] u;
178  delete[] v;
179  delete[] u2;
180  delete[] v2;
181 }
182 
184 {
185  double *u, *v;
186  u = new double[4];
187  v = new double[4];
188  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
189  u[0] = 0;
190  v[0] = 0;
191  u[1] = 160;
192  v[1] = 0;
193  u[2] = 160;
194  v[2] = 120;
195  u[3] = 0;
196  v[3] = 120;
197  // u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
198  double *u2, *v2;
199  u2 = new double[4];
200  v2 = new double[4];
201 
202  // pup=p;
203 
204  /*vpColVector ptest=pup;
205  warp(u,v,4,ptest,u2,v2);
206  for(int i=0;i<4;i++)
207  std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
208 
209  warp(u, v, 4, p, u2, v2);
210  // p=0;findWarp(u,v,u2,v2,4,p);
211 
212  for (int i = 0; i < 4; i++) {
213  u[i] = u[i] * 2.;
214  v[i] = v[i] * 2.;
215  u2[i] = u2[i] * 2.;
216  v2[i] = v2[i] * 2.;
217  /*std::cout<<"#########################################################################################"<<std::endl;
218  std::cout<<"#########################################################################################"<<std::endl;
219  std::cout<<"#########################################################################################"<<std::endl;
220  std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
221  }
222  findWarp(u, v, u2, v2, 4, pup);
223 
224  delete[] u;
225  delete[] v;
226  delete[] u2;
227  delete[] v2;
228 }
229 
231 {
232  denom = vX[0] * G[2][0] + vX[1] * G[2][1] + G[2][2];
233 }
234 
236 {
237  vpMatrix pA(3, 3);
238  pA[0][0] = p[4];
239  pA[0][1] = p[2];
240  pA[0][2] = p[0];
241 
242  pA[1][0] = p[3];
243  pA[1][1] = -p[4] - p[5];
244  pA[1][2] = p[1];
245 
246  pA[2][0] = p[6];
247  pA[2][1] = p[7];
248  pA[2][2] = p[5];
249 
250  G = pA.expm();
251 }
252 
254  const vpColVector & /*ParamM*/)
255 {
256  double i = vX[1], j = vX[0];
257  vXres[0] = (j * G[0][0] + i * G[0][1] + G[0][2]) / denom;
258  vXres[1] = (j * G[1][0] + i * G[1][1] + G[1][2]) / denom;
259 }
260 void vpTemplateTrackerWarpHomographySL3::warpX(const int &i, const int &j, double &i2, double &j2,
261  const vpColVector & /*ParamM*/)
262 {
263  j2 = (j * G[0][0] + i * G[0][1] + G[0][2]) / denom;
264  i2 = (j * G[1][0] + i * G[1][1] + G[1][2]) / denom;
265 }
266 
268 {
269  vpHomography H;
270  for (unsigned int i = 0; i < 3; i++)
271  for (unsigned int j = 0; j < 3; j++)
272  H[i][j] = G[i][j];
273  return H;
274 }
275 
277  const vpColVector & /*ParamM*/, vpMatrix &dW_)
278 {
279  vpMatrix dhdx(2, 3);
280  dhdx = 0;
281  dhdx[0][0] = 1. / denom;
282  dhdx[1][1] = 1. / denom;
283  dhdx[0][2] = -X2[0] / (denom);
284  dhdx[1][2] = -X2[1] / (denom);
285  dGx = 0;
286  for (unsigned int i = 0; i < 3; i++) {
287  dGx[i][0] = G[i][0];
288  dGx[i][1] = G[i][1];
289  dGx[i][2] = G[i][0] * X1[1];
290  dGx[i][3] = G[i][1] * X1[0];
291  dGx[i][4] = G[i][0] * X1[0] - G[i][1] * X1[1];
292  dGx[i][5] = G[i][2] - G[i][1] * X1[1];
293  dGx[i][6] = G[i][2] * X1[0];
294  dGx[i][7] = G[i][2] * X1[1];
295  }
296  dW_ = dhdx * dGx;
297 }
298 
299 /*calcul de di*dw(x,p0)/dp
300  */
301 void vpTemplateTrackerWarpHomographySL3::getdW0(const int &i, const int &j, const double &dy, const double &dx,
302  double *dIdW)
303 {
304  vpMatrix dhdx(1, 3);
305  dhdx = 0;
306  dhdx[0][0] = dx;
307  dhdx[0][1] = dy;
308  dhdx[0][2] = -j * dx - i * dy;
309  G.eye();
310 
311  dGx = 0;
312  for (unsigned int par = 0; par < 3; par++) {
313  dGx[par][0] = G[par][0];
314  dGx[par][1] = G[par][1];
315  dGx[par][2] = G[par][0] * i;
316  dGx[par][3] = G[par][1] * j;
317  dGx[par][4] = G[par][0] * j - G[par][1] * i;
318  dGx[par][5] = G[par][2] - G[par][1] * i;
319  dGx[par][6] = G[par][2] * j;
320  dGx[par][7] = G[par][2] * i;
321  }
322 
323  for (unsigned int par = 0; par < nbParam; par++) {
324  double res = 0;
325  for (unsigned int par2 = 0; par2 < 3; par2++)
326  res += dhdx[0][par2] * dGx[par2][par];
327  dIdW[par] = res;
328  }
329 }
330 /*calcul de dw(x,p0)/dp
331  */
332 
333 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &i, const int &j, double *dIdW)
334 {
335  vpMatrix dhdx(2, 3);
336  dhdx = 0;
337  dhdx[0][0] = 1.;
338  dhdx[1][1] = 1.;
339  dhdx[0][2] = -j;
340  dhdx[1][2] = -i;
341  G.eye();
342 
343  dGx = 0;
344  for (unsigned int par = 0; par < 3; par++) {
345  dGx[par][0] = G[par][0];
346  dGx[par][1] = G[par][1];
347  dGx[par][2] = G[par][0] * i;
348  dGx[par][3] = G[par][1] * j;
349  dGx[par][4] = G[par][0] * j - G[par][1] * i;
350  dGx[par][5] = G[par][2] - G[par][1] * i;
351  dGx[par][6] = G[par][2] * j;
352  dGx[par][7] = G[par][2] * i;
353  }
354  vpMatrix dIdW_temp(2, nbParam);
355  dIdW_temp = dhdx * dGx;
356 
357  for (unsigned int par = 0; par < nbParam; par++) {
358  dIdW[par] = dIdW_temp[0][par];
359  dIdW[par + nbParam] = dIdW_temp[1][par];
360  }
361 }
362 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &i, const double &j, double *dIdW)
363 {
364  vpMatrix dhdx(2, 3);
365  dhdx = 0;
366  dhdx[0][0] = 1.;
367  dhdx[1][1] = 1.;
368  dhdx[0][2] = -j;
369  dhdx[1][2] = -i;
370  G.eye();
371 
372  dGx = 0;
373  for (unsigned int par = 0; par < 3; par++) {
374  dGx[par][0] = G[par][0];
375  dGx[par][1] = G[par][1];
376  dGx[par][2] = G[par][0] * i;
377  dGx[par][3] = G[par][1] * j;
378  dGx[par][4] = G[par][0] * j - G[par][1] * i;
379  dGx[par][5] = G[par][2] - G[par][1] * i;
380  dGx[par][6] = G[par][2] * j;
381  dGx[par][7] = G[par][2] * i;
382  }
383  vpMatrix dIdW_temp(2, nbParam);
384  dIdW_temp = dhdx * dGx;
385 
386  for (unsigned int par = 0; par < nbParam; par++) {
387  dIdW[par] = dIdW_temp[0][par];
388  dIdW[par + nbParam] = dIdW_temp[1][par];
389  }
390 }
391 /*compute dw=dw/dx*dw/dp
392  */
393 
395  const vpColVector & /*ParamM*/, const double *dwdp0, vpMatrix &dW_)
396 {
397  for (unsigned int i = 0; i < nbParam; i++) {
398  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]);
399  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]);
400  }
401 }
402 
404 {
405  ParamMinv = -ParamM;
406 }
408 {
409  // vrai que si commutatif ...
410  pres = p1 + p2;
411 }
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void computeDenom(vpColVector &vX, const vpColVector &ParamM)
void getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
vpMatrix expm() const
Definition: vpMatrix.cpp:5007
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
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:71
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:174
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
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:375
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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:5193
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)
void eye()
Definition: vpMatrix.cpp:360