Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
vpTemplateTrackerWarpHomographySL3.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  *
38 *****************************************************************************/
39 #include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
40 
41 BEGIN_VISP_NAMESPACE
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  }
114  catch (const vpException &e) {
115  // std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
116  throw(e);
117  }
118  pRondp(p, dp, p);
119 
120  cpt++;
121  // std::cout<<"erreur ="<<erreur<<std::endl;
122  }
123  // while((cpt<1500));
124  while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
125 
126  // std::cout<<"erreur apres transformation="<<erreur<<std::endl;
127  for (int i = 0; i < nb_pt; i++)
128  delete[] dW_ddp0[i];
129  delete[] dW_ddp0;
130 }
131 
136 {
137  nbParam = 8;
138  G.resize(3, 3);
139  dGx.resize(3, nbParam);
140 
141  A.resize(8);
142  for (unsigned int i = 0; i < 8; i++) {
143  A[i].resize(3, 3);
144  A[i] = 0;
145  }
146  A[0][0][2] = 1;
147  A[1][1][2] = 1;
148  A[2][0][1] = 1;
149  A[3][1][0] = 1;
150  A[4][0][0] = 1;
151  A[4][1][1] = -1;
152  A[5][1][1] = -1;
153  A[5][2][2] = 1;
154  A[6][2][0] = 1;
155  A[7][2][1] = 1;
156 }
157 
158 // get the parameter corresponding to the lower level of a gaussian pyramid
159 // a refaire de facon analytique
167 {
168  double *u, *v;
169  u = new double[4];
170  v = new double[4];
171  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
172  u[0] = 0;
173  v[0] = 0;
174  u[1] = 160;
175  v[1] = 0;
176  u[2] = 160;
177  v[2] = 120;
178  u[3] = 0;
179  v[3] = 120;
180  double *u2, *v2;
181  u2 = new double[4];
182  v2 = new double[4];
183  warp(u, v, 4, p, u2, v2);
184  // p=0;findWarp(u,v,u2,v2,4,p);
185  for (int i = 0; i < 4; i++) {
186  u[i] = u[i] / 2.;
187  v[i] = v[i] / 2.;
188  u2[i] = u2[i] / 2.;
189  v2[i] = v2[i] / 2.;
190  // std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
191  }
192  p_down = p;
193  findWarp(u, v, u2, v2, 4, p_down);
194  delete[] u;
195  delete[] v;
196  delete[] u2;
197  delete[] v2;
198 }
199 
207 {
208  double *u, *v;
209  u = new double[4];
210  v = new double[4];
211  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
212  u[0] = 0;
213  v[0] = 0;
214  u[1] = 160;
215  v[1] = 0;
216  u[2] = 160;
217  v[2] = 120;
218  u[3] = 0;
219  v[3] = 120;
220  // u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
221  double *u2, *v2;
222  u2 = new double[4];
223  v2 = new double[4];
224 
225  // p_up=p;
226 
227  /*vpColVector ptest=pup;
228  warp(u,v,4,ptest,u2,v2);
229  for(int i=0;i<4;i++)
230  std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
231 
232  warp(u, v, 4, p, u2, v2);
233  // p=0;findWarp(u,v,u2,v2,4,p);
234 
235  for (int i = 0; i < 4; i++) {
236  u[i] = u[i] * 2.;
237  v[i] = v[i] * 2.;
238  u2[i] = u2[i] * 2.;
239  v2[i] = v2[i] * 2.;
240  /*std::cout<<"#########################################################################################"<<std::endl;
241  std::cout<<"#########################################################################################"<<std::endl;
242  std::cout<<"#########################################################################################"<<std::endl;
243  std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
244  }
245  findWarp(u, v, u2, v2, 4, p_up);
246 
247  delete[] u;
248  delete[] v;
249  delete[] u2;
250  delete[] v2;
251 }
252 
262 {
263  denom = X[0] * G[2][0] + X[1] * G[2][1] + G[2][2];
264 }
265 
272 {
273  vpMatrix pA(3, 3);
274  pA[0][0] = p[4];
275  pA[0][1] = p[2];
276  pA[0][2] = p[0];
277 
278  pA[1][0] = p[3];
279  pA[1][1] = -p[4] - p[5];
280  pA[1][2] = p[1];
281 
282  pA[2][0] = p[6];
283  pA[2][1] = p[7];
284  pA[2][2] = p[5];
285 
286  G = pA.expm();
287 }
288 
298 {
299  double u = X1[0], v = X1[1];
300  X2[0] = (u * G[0][0] + v * G[0][1] + G[0][2]) / denom;
301  X2[1] = (u * G[1][0] + v * G[1][1] + G[1][2]) / denom;
302 }
303 
314 void vpTemplateTrackerWarpHomographySL3::warpX(const int &v1, const int &u1, double &v2, double &u2,
315  const vpColVector &)
316 {
317  u2 = (u1 * G[0][0] + v1 * G[0][1] + G[0][2]) / denom;
318  v2 = (u1 * G[1][0] + v1 * G[1][1] + G[1][2]) / denom;
319 }
320 
326 {
327  vpHomography H;
328  for (unsigned int i = 0; i < 3; i++)
329  for (unsigned int j = 0; j < 3; j++)
330  H[i][j] = G[i][j];
331  return H;
332 }
333 
348  vpMatrix &dM)
349 {
350  vpMatrix dhdx(2, 3);
351  dhdx = 0;
352  dhdx[0][0] = 1. / denom;
353  dhdx[1][1] = 1. / denom;
354  dhdx[0][2] = -X2[0] / (denom);
355  dhdx[1][2] = -X2[1] / (denom);
356  dGx = 0;
357  for (unsigned int i = 0; i < 3; i++) {
358  dGx[i][0] = G[i][0];
359  dGx[i][1] = G[i][1];
360  dGx[i][2] = G[i][0] * X1[1];
361  dGx[i][3] = G[i][1] * X1[0];
362  dGx[i][4] = G[i][0] * X1[0] - G[i][1] * X1[1];
363  dGx[i][5] = G[i][2] - G[i][1] * X1[1];
364  dGx[i][6] = G[i][2] * X1[0];
365  dGx[i][7] = G[i][2] * X1[1];
366  }
367  dM = dhdx * dGx;
368 }
369 
378 void vpTemplateTrackerWarpHomographySL3::getdW0(const int &v, const int &u, const double &dv, const double &du,
379  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 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 
526 {
527  // vrai que si commutatif ...
528  p12 = p1 + p2;
529 }
530 END_VISP_NAMESPACE
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix expm() const
Definition: vpMatrix.cpp:1669
vpMatrix inverseByLU() const
vpMatrix AtA() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:1869
vpMatrix t() const
void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
void computeDenom(vpColVector &X, const vpColVector &)
void warpX(const vpColVector &X1, vpColVector &X2, const vpColVector &)
void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const
void dWarpCompo(const vpColVector &, const vpColVector &X, const vpColVector &, const double *dwdp0, vpMatrix &dW)
void getParamPyramidDown(const vpColVector &p, vpColVector &p_down)
void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p)
void getdWdp0(const int &v, const int &u, double *dIdW)
void getParamInverse(const vpColVector &p, vpColVector &p_inv) const
void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &, vpMatrix &dW)
void getParamPyramidUp(const vpColVector &p, vpColVector &p_up)
unsigned int nbParam
Number of parameters used to model warp transformation.
double denom
Internal value used by homography warp model.
void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v)