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
45 /*!
46 * Find the displacement/warping function parameters from a list of points.
47 *
48 * \param ut0 : Original u coordinates.
49 * \param vt0 : Original v coordinates.
50 * \param u : Warped u coordinates.
51 * \param v : Warped v coordinates.
52 * \param nb_pt : Number of points.
53 * \param p : Resulting warping function parameters.
54 */
findWarp(const double * ut0,const double * vt0,const double * u,const double * v,int nb_pt,vpColVector & p)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);
61 vpMatrix H(nbParam, nbParam), HLM(nbParam, nbParam);
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
131 /*!
132 * Construct an homography SL3 model with 8 parameters initialized to zero.
133 */
vpTemplateTrackerWarpHomographySL3()134 vpTemplateTrackerWarpHomographySL3::vpTemplateTrackerWarpHomographySL3() : G(), dGx(), A()
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
~vpTemplateTrackerWarpHomographySL3()157 vpTemplateTrackerWarpHomographySL3::~vpTemplateTrackerWarpHomographySL3() {}
158
159 // get the parameter corresponding to the lower level of a gaussian pyramid
160 // a refaire de facon analytique
161 /*!
162 * Get the parameters of the warping function one level down
163 * where image size is divided by two along the lines and the columns.
164 * \param p : 8-dim vector that contains the current parameters of the warping function.
165 * \param p_down : 8-dim vector that contains the resulting parameters one level down.
166 */
getParamPyramidDown(const vpColVector & p,vpColVector & p_down)167 void vpTemplateTrackerWarpHomographySL3::getParamPyramidDown(const vpColVector &p, vpColVector &p_down)
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
201 /*!
202 * Get the parameters of the warping function one level up
203 * where image size is multiplied by two along the lines and the columns.
204 * \param p : 8-dim vector that contains the current parameters of the warping function.
205 * \param p_up : 8-dim vector that contains the resulting parameters one level up.
206 */
getParamPyramidUp(const vpColVector & p,vpColVector & p_up)207 void vpTemplateTrackerWarpHomographySL3::getParamPyramidUp(const vpColVector &p, vpColVector &p_up)
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
254 /*!
255 * Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
256 * \param X : Point with coordinates (u, v) to consider.
257 *
258 * \sa warpX(const vpColVector &, vpColVector &, const vpColVector &)
259 * \sa warpX(const int &, const int &, double &, double &, const vpColVector &)
260 * \sa dWarp(), dWarpCompo()
261 */
computeDenom(vpColVector & X,const vpColVector &)262 void vpTemplateTrackerWarpHomographySL3::computeDenom(vpColVector &X, const vpColVector &)
263 {
264 denom = X[0] * G[2][0] + X[1] * G[2][1] + G[2][2];
265 }
266
267 /*!
268 * Compute the exponential of the homography matrix defined by the given
269 * parameters.
270 * \param p : Parameters of the SL3 homography warping function.
271 */
computeCoeff(const vpColVector & p)272 void vpTemplateTrackerWarpHomographySL3::computeCoeff(const vpColVector &p)
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
290 /*!
291 * Warp point \f$X_1=(u_1,v_1)\f$ using the transformation model.
292 * \f[X_2 = {^2}M_1(p) * X_1\f]
293 * \param X1 : 2-dim vector corresponding to the coordinates \f$(u_1, v_1)\f$ of the point to warp.
294 * \param X2 : 2-dim vector corresponding to the coordinates \f$(u_2, v_2)\f$ of the warped point.
295 *
296 * \sa computeDenom()
297 */
warpX(const vpColVector & X1,vpColVector & X2,const vpColVector &)298 void vpTemplateTrackerWarpHomographySL3::warpX(const vpColVector &X1, vpColVector &X2,
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
306 /*!
307 * Warp point \f$X_1=(u_1,v_1)\f$ using the transformation model with parameters \f$p\f$.
308 * \f[X_2 = {^2}M_1(p) * X_1\f]
309 * \param v1 : Coordinate (along the image rows axis) of the point \f$X_1=(u_1,v_1)\f$ to warp.
310 * \param u1 : Coordinate (along the image columns axis) of the point \f$X_1=(u_1,v_1)\f$ to warp.
311 * \param v2 : Coordinate of the warped point \f$X_2=(u_2,v_2)\f$ along the image rows axis.
312 * \param u2 : Coordinate of the warped point \f$X_2=(u_2,v_2)\f$ along the image column axis.
313 *
314 * \sa computeDenom()
315 */
warpX(const int & v1,const int & u1,double & v2,double & u2,const vpColVector &)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
322 /*!
323 * Return the homography corresponding to the parameters.
324 * \return Corresponding homography.
325 */
getHomography() const326 vpHomography vpTemplateTrackerWarpHomographySL3::getHomography() const
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
335 /*!
336 * Compute the derivative matrix of the warping function at point \f$X=(u,v)\f$ according to the model parameters:
337 * \f[
338 * \frac{\partial M}{\partial p}(X, p)
339 * \f]
340 * \param X1 : 2-dim vector corresponding to the coordinates \f$(u_1, v_1)\f$ of the point to
341 * consider in the derivative computation.
342 * \param X2 : 2-dim vector corresponding to the coordinates \f$(u_2, v_2)\f$ of the point to
343 * consider in the derivative computation.
344 * \param dM : Resulting warping model derivative returned as a 2-by-8 matrix.
345 *
346 * \sa computeDenom()
347 */
dWarp(const vpColVector & X1,const vpColVector & X2,const vpColVector &,vpMatrix & dM)348 void vpTemplateTrackerWarpHomographySL3::dWarp(const vpColVector &X1, const vpColVector &X2,
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
371 /*!
372 * Compute the derivative of the image with relation to the warping function parameters.
373 * \param v : Coordinate (along the image rows axis) of the point to consider in the image.
374 * \param u : Coordinate (along the image columns axis) of the point to consider in the image.
375 * \param dv : Derivative on the v-axis (along the rows) of the point (u,v).
376 * \param du : Derivative on the u-axis (along the columns) of the point (u,v).
377 * \param dIdW : Resulting derivative matrix (image according to the warping function).
378 */
getdW0(const int & v,const int & u,const double & dv,const double & du,double * dIdW)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 }
407 /*!
408 * Compute the derivative of the warping model \f$M\f$ according to the initial parameters \f$p_0\f$
409 * at point \f$X=(u,v)\f$:
410 * \f[
411 * \frac{\partial M}{\partial p}(X, p_0)
412 * \f]
413 *
414 * \param v : Coordinate (along the image rows axis) of the point X(u,v) to consider in the image.
415 * \param u : Coordinate (along the image columns axis) of the point X(u,v) to consider in the image.
416 * \param dIdW : Resulting 2-by-8 derivative matrix.
417 */
getdWdp0(const int & v,const int & u,double * dIdW)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
448 /*!
449 * Compute the derivative of the warping model \f$M\f$ according to the initial parameters \f$p_0\f$
450 * at point \f$X=(u,v)\f$:
451 * \f[
452 * \frac{\partial M}{\partial p}(X, p_0)
453 * \f]
454 *
455 * \param v : Coordinate (along the image rows axis) of the point X(u,v) to consider in the image.
456 * \param u : Coordinate (along the image columns axis) of the point X(u,v) to consider in the image.
457 * \param dIdW : Resulting 2-by-8 derivative matrix.
458 */
getdWdp0(const double & v,const double & u,double * dIdW)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
489 /*!
490 * Compute the compositionnal derivative matrix of the warping function according to the model parameters.
491 * \param X : 2-dim vector corresponding to the coordinates \f$(u_1, v_1)\f$ of the point to
492 * consider in the derivative computation.
493 * \param dwdp0 : 2-by-8 derivative matrix of the warping function according to
494 * the initial warping function parameters (p=0).
495 * \param dM : Resulting warping model compositionnal derivative returned as a 2-by-8 matrix.
496 *
497 * \sa computeDenom()
498 */
499
dWarpCompo(const vpColVector &,const vpColVector & X,const vpColVector &,const double * dwdp0,vpMatrix & dM)500 void vpTemplateTrackerWarpHomographySL3::dWarpCompo(const vpColVector &, const vpColVector &X,
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
509 /*!
510 * Compute inverse of the warping transformation.
511 * \param p : 8-dim vector that contains the parameters corresponding
512 * to the transformation to inverse.
513 * \param p_inv : 8-dim vector that contains the parameters of the inverse transformation \f$ {M(p)}^{-1}\f$.
514 */
getParamInverse(const vpColVector & p,vpColVector & p_inv) const515 void vpTemplateTrackerWarpHomographySL3::getParamInverse(const vpColVector &p, vpColVector &p_inv) const
516 {
517 p_inv = -p;
518 }
519
520 /*!
521 * Compute the transformation resulting from the composition of two other transformations.
522 * \param p1 : 8-dim vector that contains the parameters corresponding
523 * to first transformation.
524 * \param p2 : 8-dim vector that contains the parameters corresponding
525 * to second transformation.
526 * \param p12 : 8-dim vector that contains the resulting transformation \f$ p_{12} = p_1 \circ p_2\f$.
527 */
pRondp(const vpColVector & p1,const vpColVector & p2,vpColVector & p12) const528 void vpTemplateTrackerWarpHomographySL3::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const
529 {
530 // vrai que si commutatif ...
531 p12 = p1 + p2;
532 }
533