Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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
55 void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0, const double *vt0, const double *u,
56  const double *v, int nb_pt, vpColVector &p)
57 {
58  vpColVector dp(nbParam);
59  vpMatrix dW_(2, nbParam);
60  vpMatrix dX(2, 1);
62  vpMatrix G_(nbParam, 1);
63 
64  // vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
65  double **dW_ddp0 = new double *[(unsigned int)nb_pt];
66  for (int i = 0; i < nb_pt; i++) {
67  // dW_ddp0[i].resize(2,nbParam);
68  dW_ddp0[i] = new double[2 * nbParam];
69  // getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
70  // std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
71  getdWdp0(v[i], u[i], dW_ddp0[i]);
72  }
73 
74  int cpt = 0;
75  vpColVector X1(2);
76  vpColVector fX1(2);
77  vpColVector X2(2);
78  double erreur = 0;
79  double erreur_prec;
80  double lambda = 0.00001;
81  do {
82  erreur_prec = erreur;
83  H = 0;
84  G_ = 0;
85  erreur = 0;
86  computeCoeff(p);
87  for (int i = 0; i < nb_pt; i++) {
88  X1[0] = ut0[i];
89  X1[1] = vt0[i];
90  computeDenom(X1, p);
91  warpX(X1, fX1, p);
92  // dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
93  // dWarp(X1,fX1,p,dW);
94  for (unsigned int ip = 0; ip < nbParam; ip++) {
95  dW_[0][ip] = dW_ddp0[i][ip];
96  dW_[1][ip] = dW_ddp0[i][ip + nbParam];
97  }
98 
99  H += dW_.AtA();
100 
101  X2[0] = u[i];
102  X2[1] = v[i];
103 
104  dX = X2 - fX1;
105  G_ += dW_.t() * dX;
106 
107  erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + (v[i] - fX1[1]) * (v[i] - fX1[1]));
108  }
109 
110  vpMatrix::computeHLM(H, lambda, HLM);
111  try {
112  dp = HLM.inverseByLU() * G_;
113  } catch (const vpException &e) {
114  // std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
115  throw(e);
116  }
117  pRondp(p, dp, p);
118 
119  cpt++;
120  // std::cout<<"erreur ="<<erreur<<std::endl;
121  }
122  // while((cpt<1500));
123  while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
124 
125  // std::cout<<"erreur apres transformation="<<erreur<<std::endl;
126  for (int i = 0; i < nb_pt; i++)
127  delete[] dW_ddp0[i];
128  delete[] dW_ddp0;
129 }
130 
135 {
136  nbParam = 8;
137  G.resize(3, 3);
138  dGx.resize(3, nbParam);
139 
140  A.resize(8);
141  for (unsigned int i = 0; i < 8; i++) {
142  A[i].resize(3, 3);
143  A[i] = 0;
144  }
145  A[0][0][2] = 1;
146  A[1][1][2] = 1;
147  A[2][0][1] = 1;
148  A[3][1][0] = 1;
149  A[4][0][0] = 1;
150  A[4][1][1] = -1;
151  A[5][1][1] = -1;
152  A[5][2][2] = 1;
153  A[6][2][0] = 1;
154  A[7][2][1] = 1;
155 }
156 
158 
159 // get the parameter corresponding to the lower level of a gaussian pyramid
160 // a refaire de facon analytique
168 {
169  double *u, *v;
170  u = new double[4];
171  v = new double[4];
172  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
173  u[0] = 0;
174  v[0] = 0;
175  u[1] = 160;
176  v[1] = 0;
177  u[2] = 160;
178  v[2] = 120;
179  u[3] = 0;
180  v[3] = 120;
181  double *u2, *v2;
182  u2 = new double[4];
183  v2 = new double[4];
184  warp(u, v, 4, p, u2, v2);
185  // p=0;findWarp(u,v,u2,v2,4,p);
186  for (int i = 0; i < 4; i++) {
187  u[i] = u[i] / 2.;
188  v[i] = v[i] / 2.;
189  u2[i] = u2[i] / 2.;
190  v2[i] = v2[i] / 2.;
191  // std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
192  }
193  p_down = p;
194  findWarp(u, v, u2, v2, 4, p_down);
195  delete[] u;
196  delete[] v;
197  delete[] u2;
198  delete[] v2;
199 }
200 
208 {
209  double *u, *v;
210  u = new double[4];
211  v = new double[4];
212  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
213  u[0] = 0;
214  v[0] = 0;
215  u[1] = 160;
216  v[1] = 0;
217  u[2] = 160;
218  v[2] = 120;
219  u[3] = 0;
220  v[3] = 120;
221  // u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
222  double *u2, *v2;
223  u2 = new double[4];
224  v2 = new double[4];
225 
226  // p_up=p;
227 
228  /*vpColVector ptest=pup;
229  warp(u,v,4,ptest,u2,v2);
230  for(int i=0;i<4;i++)
231  std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
232 
233  warp(u, v, 4, p, u2, v2);
234  // p=0;findWarp(u,v,u2,v2,4,p);
235 
236  for (int i = 0; i < 4; i++) {
237  u[i] = u[i] * 2.;
238  v[i] = v[i] * 2.;
239  u2[i] = u2[i] * 2.;
240  v2[i] = v2[i] * 2.;
241  /*std::cout<<"#########################################################################################"<<std::endl;
242  std::cout<<"#########################################################################################"<<std::endl;
243  std::cout<<"#########################################################################################"<<std::endl;
244  std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
245  }
246  findWarp(u, v, u2, v2, 4, p_up);
247 
248  delete[] u;
249  delete[] v;
250  delete[] u2;
251  delete[] v2;
252 }
253 
263 {
264  denom = X[0] * G[2][0] + X[1] * G[2][1] + G[2][2];
265 }
266 
273 {
274  vpMatrix pA(3, 3);
275  pA[0][0] = p[4];
276  pA[0][1] = p[2];
277  pA[0][2] = p[0];
278 
279  pA[1][0] = p[3];
280  pA[1][1] = -p[4] - p[5];
281  pA[1][2] = p[1];
282 
283  pA[2][0] = p[6];
284  pA[2][1] = p[7];
285  pA[2][2] = p[5];
286 
287  G = pA.expm();
288 }
289 
299  const vpColVector &)
300 {
301  double u = X1[0], v = X1[1];
302  X2[0] = (u * G[0][0] + v * G[0][1] + G[0][2]) / denom;
303  X2[1] = (u * G[1][0] + v * G[1][1] + G[1][2]) / denom;
304 }
305 
316 void vpTemplateTrackerWarpHomographySL3::warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &)
317 {
318  u2 = (u1 * G[0][0] + v1 * G[0][1] + G[0][2]) / denom;
319  v2 = (u1 * G[1][0] + v1 * G[1][1] + G[1][2]) / denom;
320 }
321 
327 {
328  vpHomography H;
329  for (unsigned int i = 0; i < 3; i++)
330  for (unsigned int j = 0; j < 3; j++)
331  H[i][j] = G[i][j];
332  return H;
333 }
334 
349  const vpColVector &, vpMatrix &dM)
350 {
351  vpMatrix dhdx(2, 3);
352  dhdx = 0;
353  dhdx[0][0] = 1. / denom;
354  dhdx[1][1] = 1. / denom;
355  dhdx[0][2] = -X2[0] / (denom);
356  dhdx[1][2] = -X2[1] / (denom);
357  dGx = 0;
358  for (unsigned int i = 0; i < 3; i++) {
359  dGx[i][0] = G[i][0];
360  dGx[i][1] = G[i][1];
361  dGx[i][2] = G[i][0] * X1[1];
362  dGx[i][3] = G[i][1] * X1[0];
363  dGx[i][4] = G[i][0] * X1[0] - G[i][1] * X1[1];
364  dGx[i][5] = G[i][2] - G[i][1] * X1[1];
365  dGx[i][6] = G[i][2] * X1[0];
366  dGx[i][7] = G[i][2] * X1[1];
367  }
368  dM = dhdx * dGx;
369 }
370 
379 void vpTemplateTrackerWarpHomographySL3::getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
380 {
381  vpMatrix dhdx(1, 3);
382  dhdx = 0;
383  dhdx[0][0] = du;
384  dhdx[0][1] = dv;
385  dhdx[0][2] = -u * du - v * dv;
386  G.eye();
387 
388  dGx = 0;
389  for (unsigned int par = 0; par < 3; par++) {
390  dGx[par][0] = G[par][0];
391  dGx[par][1] = G[par][1];
392  dGx[par][2] = G[par][0] * v;
393  dGx[par][3] = G[par][1] * u;
394  dGx[par][4] = G[par][0] * u - G[par][1] * v;
395  dGx[par][5] = G[par][2] - G[par][1] * v;
396  dGx[par][6] = G[par][2] * u;
397  dGx[par][7] = G[par][2] * v;
398  }
399 
400  for (unsigned int par = 0; par < nbParam; par++) {
401  double res = 0;
402  for (unsigned int par2 = 0; par2 < 3; par2++)
403  res += dhdx[0][par2] * dGx[par2][par];
404  dIdW[par] = res;
405  }
406 }
418 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &v, const int &u, double *dIdW)
419 {
420  vpMatrix dhdx(2, 3);
421  dhdx = 0;
422  dhdx[0][0] = 1.;
423  dhdx[1][1] = 1.;
424  dhdx[0][2] = -u;
425  dhdx[1][2] = -v;
426  G.eye();
427 
428  dGx = 0;
429  for (unsigned int par = 0; par < 3; par++) {
430  dGx[par][0] = G[par][0];
431  dGx[par][1] = G[par][1];
432  dGx[par][2] = G[par][0] * v;
433  dGx[par][3] = G[par][1] * u;
434  dGx[par][4] = G[par][0] * u - G[par][1] * v;
435  dGx[par][5] = G[par][2] - G[par][1] * v;
436  dGx[par][6] = G[par][2] * u;
437  dGx[par][7] = G[par][2] * v;
438  }
439  vpMatrix dIdW_temp(2, nbParam);
440  dIdW_temp = dhdx * dGx;
441 
442  for (unsigned int par = 0; par < nbParam; par++) {
443  dIdW[par] = dIdW_temp[0][par];
444  dIdW[par + nbParam] = dIdW_temp[1][par];
445  }
446 }
447 
459 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &v, const double &u, double *dIdW)
460 {
461  vpMatrix dhdx(2, 3);
462  dhdx = 0;
463  dhdx[0][0] = 1.;
464  dhdx[1][1] = 1.;
465  dhdx[0][2] = -u;
466  dhdx[1][2] = -v;
467  G.eye();
468 
469  dGx = 0;
470  for (unsigned int par = 0; par < 3; par++) {
471  dGx[par][0] = G[par][0];
472  dGx[par][1] = G[par][1];
473  dGx[par][2] = G[par][0] * v;
474  dGx[par][3] = G[par][1] * u;
475  dGx[par][4] = G[par][0] * u - G[par][1] * v;
476  dGx[par][5] = G[par][2] - G[par][1] * v;
477  dGx[par][6] = G[par][2] * u;
478  dGx[par][7] = G[par][2] * v;
479  }
480  vpMatrix dIdW_temp(2, nbParam);
481  dIdW_temp = dhdx * dGx;
482 
483  for (unsigned int par = 0; par < nbParam; par++) {
484  dIdW[par] = dIdW_temp[0][par];
485  dIdW[par + nbParam] = dIdW_temp[1][par];
486  }
487 }
488 
501  const vpColVector &, const double *dwdp0, vpMatrix &dM)
502 {
503  for (unsigned int i = 0; i < nbParam; i++) {
504  dM[0][i] = denom * ((G[0][0] - X[0] * G[2][0]) * dwdp0[i] + (G[0][1] - X[0] * G[2][1]) * dwdp0[i + nbParam]);
505  dM[1][i] = denom * ((G[1][0] - X[1] * G[2][0]) * dwdp0[i] + (G[1][1] - X[1] * G[2][1]) * dwdp0[i + nbParam]);
506  }
507 }
508 
516 {
517  p_inv = -p;
518 }
519 
529 {
530  // vrai que si commutatif ...
531  p12 = p1 + p2;
532 }
void getParamPyramidDown(const vpColVector &p, vpColVector &p_down)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
void computeDenom(vpColVector &X, const vpColVector &)
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
void getdWdp0(const int &v, const int &u, double *dIdW)
vpMatrix inverseByLU() const
void getParamInverse(const vpColVector &p, vpColVector &p_inv) const
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)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpMatrix t() const
Definition: vpMatrix.cpp:464
void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const
double denom
Internal value used by homography warp model.
void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &, vpMatrix &dW)
void warpX(const vpColVector &X1, vpColVector &X2, const vpColVector &)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void getParamPyramidUp(const vpColVector &p, vpColVector &p_up)
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)
void eye()
Definition: vpMatrix.cpp:449
unsigned int nbParam
Number of parameters used to model warp transformation.
void dWarpCompo(const vpColVector &, const vpColVector &X, const vpColVector &, const double *dwdp0, vpMatrix &dW)
vpMatrix expm() const
Definition: vpMatrix.cpp:6494