Visual Servoing Platform version 3.5.0
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
55void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0, const double *vt0, const double *u,
56 const double *v, int nb_pt, vpColVector &p)
57{
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
316void 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
379void 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}
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 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 resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix expm() const
Definition: vpMatrix.cpp:6494
void eye()
Definition: vpMatrix.cpp:449
vpMatrix inverseByLU() const
vpMatrix t() const
Definition: vpMatrix.cpp:464
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
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)