Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
54void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0, const double *vt0, const double *u,
55 const double *v, int nb_pt, vpColVector &p)
56{
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 } catch (const vpException &e) {
113 // std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
114 throw(e);
115 }
116 pRondp(p, dp, p);
117
118 cpt++;
119 // std::cout<<"erreur ="<<erreur<<std::endl;
120 }
121 // while((cpt<1500));
122 while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
123
124 // std::cout<<"erreur apres transformation="<<erreur<<std::endl;
125 for (int i = 0; i < nb_pt; i++)
126 delete[] dW_ddp0[i];
127 delete[] dW_ddp0;
128}
129
134{
135 nbParam = 8;
136 G.resize(3, 3);
137 dGx.resize(3, nbParam);
138
139 A.resize(8);
140 for (unsigned int i = 0; i < 8; i++) {
141 A[i].resize(3, 3);
142 A[i] = 0;
143 }
144 A[0][0][2] = 1;
145 A[1][1][2] = 1;
146 A[2][0][1] = 1;
147 A[3][1][0] = 1;
148 A[4][0][0] = 1;
149 A[4][1][1] = -1;
150 A[5][1][1] = -1;
151 A[5][2][2] = 1;
152 A[6][2][0] = 1;
153 A[7][2][1] = 1;
154}
155
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
314void 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
378void 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}
418void 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
459void 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}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
Implementation of an homography and operations on homographies.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix expm() const
void eye()
Definition vpMatrix.cpp:446
vpMatrix inverseByLU() const
vpMatrix t() const
Definition vpMatrix.cpp:461
vpMatrix AtA() const
Definition vpMatrix.cpp:625
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
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)