Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 // findWarp special a SL3 car methode additionnelle ne marche pas (la derivee
42 // n est calculable qu en p=0)
43 // => resout le probleme de maniere compositionnelle
54 void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0, const double *vt0, const double *u,
55  const double *v, int nb_pt, vpColVector &p)
56 {
57  vpColVector dp(nbParam);
58  vpMatrix dW_(2, nbParam);
59  vpMatrix dX(2, 1);
61  vpMatrix G_(nbParam, 1);
62 
63  // vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
64  double **dW_ddp0 = new double *[(unsigned int)nb_pt];
65  for (int i = 0; i < nb_pt; i++) {
66  // dW_ddp0[i].resize(2,nbParam);
67  dW_ddp0[i] = new double[2 * nbParam];
68  // getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
69  // std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
70  getdWdp0(v[i], u[i], dW_ddp0[i]);
71  }
72 
73  int cpt = 0;
74  vpColVector X1(2);
75  vpColVector fX1(2);
76  vpColVector X2(2);
77  double erreur = 0;
78  double erreur_prec;
79  double lambda = 0.00001;
80  do {
81  erreur_prec = erreur;
82  H = 0;
83  G_ = 0;
84  erreur = 0;
85  computeCoeff(p);
86  for (int i = 0; i < nb_pt; i++) {
87  X1[0] = ut0[i];
88  X1[1] = vt0[i];
89  computeDenom(X1, p);
90  warpX(X1, fX1, p);
91  // dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
92  // dWarp(X1,fX1,p,dW);
93  for (unsigned int ip = 0; ip < nbParam; ip++) {
94  dW_[0][ip] = dW_ddp0[i][ip];
95  dW_[1][ip] = dW_ddp0[i][ip + nbParam];
96  }
97 
98  H += dW_.AtA();
99 
100  X2[0] = u[i];
101  X2[1] = v[i];
102 
103  dX = X2 - fX1;
104  G_ += dW_.t() * dX;
105 
106  erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + (v[i] - fX1[1]) * (v[i] - fX1[1]));
107  }
108 
109  vpMatrix::computeHLM(H, lambda, HLM);
110  try {
111  dp = HLM.inverseByLU() * G_;
112  }
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 
157 // get the parameter corresponding to the lower level of a gaussian pyramid
158 // a refaire de facon analytique
166 {
167  double *u, *v;
168  u = new double[4];
169  v = new double[4];
170  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
171  u[0] = 0;
172  v[0] = 0;
173  u[1] = 160;
174  v[1] = 0;
175  u[2] = 160;
176  v[2] = 120;
177  u[3] = 0;
178  v[3] = 120;
179  double *u2, *v2;
180  u2 = new double[4];
181  v2 = new double[4];
182  warp(u, v, 4, p, u2, v2);
183  // p=0;findWarp(u,v,u2,v2,4,p);
184  for (int i = 0; i < 4; i++) {
185  u[i] = u[i] / 2.;
186  v[i] = v[i] / 2.;
187  u2[i] = u2[i] / 2.;
188  v2[i] = v2[i] / 2.;
189  // std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
190  }
191  p_down = p;
192  findWarp(u, v, u2, v2, 4, p_down);
193  delete[] u;
194  delete[] v;
195  delete[] u2;
196  delete[] v2;
197 }
198 
206 {
207  double *u, *v;
208  u = new double[4];
209  v = new double[4];
210  // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
211  u[0] = 0;
212  v[0] = 0;
213  u[1] = 160;
214  v[1] = 0;
215  u[2] = 160;
216  v[2] = 120;
217  u[3] = 0;
218  v[3] = 120;
219  // u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
220  double *u2, *v2;
221  u2 = new double[4];
222  v2 = new double[4];
223 
224  // p_up=p;
225 
226  /*vpColVector ptest=pup;
227  warp(u,v,4,ptest,u2,v2);
228  for(int i=0;i<4;i++)
229  std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
230 
231  warp(u, v, 4, p, u2, v2);
232  // p=0;findWarp(u,v,u2,v2,4,p);
233 
234  for (int i = 0; i < 4; i++) {
235  u[i] = u[i] * 2.;
236  v[i] = v[i] * 2.;
237  u2[i] = u2[i] * 2.;
238  v2[i] = v2[i] * 2.;
239  /*std::cout<<"#########################################################################################"<<std::endl;
240  std::cout<<"#########################################################################################"<<std::endl;
241  std::cout<<"#########################################################################################"<<std::endl;
242  std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
243  }
244  findWarp(u, v, u2, v2, 4, p_up);
245 
246  delete[] u;
247  delete[] v;
248  delete[] u2;
249  delete[] v2;
250 }
251 
261 {
262  denom = X[0] * G[2][0] + X[1] * G[2][1] + G[2][2];
263 }
264 
271 {
272  vpMatrix pA(3, 3);
273  pA[0][0] = p[4];
274  pA[0][1] = p[2];
275  pA[0][2] = p[0];
276 
277  pA[1][0] = p[3];
278  pA[1][1] = -p[4] - p[5];
279  pA[1][2] = p[1];
280 
281  pA[2][0] = p[6];
282  pA[2][1] = p[7];
283  pA[2][2] = p[5];
284 
285  G = pA.expm();
286 }
287 
297 {
298  double u = X1[0], v = X1[1];
299  X2[0] = (u * G[0][0] + v * G[0][1] + G[0][2]) / denom;
300  X2[1] = (u * G[1][0] + v * G[1][1] + G[1][2]) / denom;
301 }
302 
313 void vpTemplateTrackerWarpHomographySL3::warpX(const int &v1, const int &u1, double &v2, double &u2,
314  const vpColVector &)
315 {
316  u2 = (u1 * G[0][0] + v1 * G[0][1] + G[0][2]) / denom;
317  v2 = (u1 * G[1][0] + v1 * G[1][1] + G[1][2]) / denom;
318 }
319 
325 {
326  vpHomography H;
327  for (unsigned int i = 0; i < 3; i++)
328  for (unsigned int j = 0; j < 3; j++)
329  H[i][j] = G[i][j];
330  return H;
331 }
332 
347  vpMatrix &dM)
348 {
349  vpMatrix dhdx(2, 3);
350  dhdx = 0;
351  dhdx[0][0] = 1. / denom;
352  dhdx[1][1] = 1. / denom;
353  dhdx[0][2] = -X2[0] / (denom);
354  dhdx[1][2] = -X2[1] / (denom);
355  dGx = 0;
356  for (unsigned int i = 0; i < 3; i++) {
357  dGx[i][0] = G[i][0];
358  dGx[i][1] = G[i][1];
359  dGx[i][2] = G[i][0] * X1[1];
360  dGx[i][3] = G[i][1] * X1[0];
361  dGx[i][4] = G[i][0] * X1[0] - G[i][1] * X1[1];
362  dGx[i][5] = G[i][2] - G[i][1] * X1[1];
363  dGx[i][6] = G[i][2] * X1[0];
364  dGx[i][7] = G[i][2] * X1[1];
365  }
366  dM = dhdx * dGx;
367 }
368 
377 void vpTemplateTrackerWarpHomographySL3::getdW0(const int &v, const int &u, const double &dv, const double &du,
378  double *dIdW)
379 {
380  vpMatrix dhdx(1, 3);
381  dhdx = 0;
382  dhdx[0][0] = du;
383  dhdx[0][1] = dv;
384  dhdx[0][2] = -u * du - v * dv;
385  G.eye();
386 
387  dGx = 0;
388  for (unsigned int par = 0; par < 3; par++) {
389  dGx[par][0] = G[par][0];
390  dGx[par][1] = G[par][1];
391  dGx[par][2] = G[par][0] * v;
392  dGx[par][3] = G[par][1] * u;
393  dGx[par][4] = G[par][0] * u - G[par][1] * v;
394  dGx[par][5] = G[par][2] - G[par][1] * v;
395  dGx[par][6] = G[par][2] * u;
396  dGx[par][7] = G[par][2] * v;
397  }
398 
399  for (unsigned int par = 0; par < nbParam; par++) {
400  double res = 0;
401  for (unsigned int par2 = 0; par2 < 3; par2++)
402  res += dhdx[0][par2] * dGx[par2][par];
403  dIdW[par] = res;
404  }
405 }
417 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &v, const int &u, double *dIdW)
418 {
419  vpMatrix dhdx(2, 3);
420  dhdx = 0;
421  dhdx[0][0] = 1.;
422  dhdx[1][1] = 1.;
423  dhdx[0][2] = -u;
424  dhdx[1][2] = -v;
425  G.eye();
426 
427  dGx = 0;
428  for (unsigned int par = 0; par < 3; par++) {
429  dGx[par][0] = G[par][0];
430  dGx[par][1] = G[par][1];
431  dGx[par][2] = G[par][0] * v;
432  dGx[par][3] = G[par][1] * u;
433  dGx[par][4] = G[par][0] * u - G[par][1] * v;
434  dGx[par][5] = G[par][2] - G[par][1] * v;
435  dGx[par][6] = G[par][2] * u;
436  dGx[par][7] = G[par][2] * v;
437  }
438  vpMatrix dIdW_temp(2, nbParam);
439  dIdW_temp = dhdx * dGx;
440 
441  for (unsigned int par = 0; par < nbParam; par++) {
442  dIdW[par] = dIdW_temp[0][par];
443  dIdW[par + nbParam] = dIdW_temp[1][par];
444  }
445 }
446 
458 void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &v, const double &u, double *dIdW)
459 {
460  vpMatrix dhdx(2, 3);
461  dhdx = 0;
462  dhdx[0][0] = 1.;
463  dhdx[1][1] = 1.;
464  dhdx[0][2] = -u;
465  dhdx[1][2] = -v;
466  G.eye();
467 
468  dGx = 0;
469  for (unsigned int par = 0; par < 3; par++) {
470  dGx[par][0] = G[par][0];
471  dGx[par][1] = G[par][1];
472  dGx[par][2] = G[par][0] * v;
473  dGx[par][3] = G[par][1] * u;
474  dGx[par][4] = G[par][0] * u - G[par][1] * v;
475  dGx[par][5] = G[par][2] - G[par][1] * v;
476  dGx[par][6] = G[par][2] * u;
477  dGx[par][7] = G[par][2] * v;
478  }
479  vpMatrix dIdW_temp(2, nbParam);
480  dIdW_temp = dhdx * dGx;
481 
482  for (unsigned int par = 0; par < nbParam; par++) {
483  dIdW[par] = dIdW_temp[0][par];
484  dIdW[par + nbParam] = dIdW_temp[1][par];
485  }
486 }
487 
500  const double *dwdp0, vpMatrix &dM)
501 {
502  for (unsigned int i = 0; i < nbParam; i++) {
503  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]);
504  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]);
505  }
506 }
507 
515 
525 {
526  // vrai que si commutatif ...
527  p12 = p1 + p2;
528 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:168
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix expm() const
Definition: vpMatrix.cpp:6481
void eye()
Definition: vpMatrix.cpp:442
vpMatrix inverseByLU() const
vpMatrix t() const
Definition: vpMatrix.cpp:457
vpMatrix AtA() const
Definition: vpMatrix.cpp:633
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6671
void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)
void computeDenom(vpColVector &X, const vpColVector &)
void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p)
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 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)