Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpHomographyMalis.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  * Homography estimation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
50 #include <visp3/core/vpDebug.h>
51 #include <visp3/core/vpMatrixException.h>
52 #include <visp3/vision/vpHomography.h>
53 
54 #include <cmath> // std::fabs
55 #include <limits> // numeric_limits
56 
57 #ifndef DOXYGEN_SHOULD_SKIP_THIS
58 const double eps = 1e-6;
59 
60 /**************************************************************************
61  * NOM :
62  * changeFrame
63  *
64  * DESCRIPTION :
65  * Changement de repere Euclidien.
66  *
67  ****************************************************************************
68  * ENTREES :
69  * int pts_ref[4] : Definit quels sont les points de reference, ils ne
70  * seront pas affectes par le changement de repere
71  * int nb_pts : nombre de points a changer de repere
72  * double **pd : La matrice des coordonnees des points desires
73  * double **p : La matrice des coordonnees des points courants
74  *
75  *
76  * SORTIES :
77  *
78  * double **pt_des_nr : La matrice des coordonnees des points desires
79  * dans le nouveau repere.
80  * double **pt_cour_nr : La matrice des coordonnees des points courants
81  * dans le nouveau repere
82  * double **M : ??
83  * double **Mpd : pseudo inverse de M ..
84  *
85  *
86  ****************************************************************************
87  */
88 
89 void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &pnd, vpMatrix &pn,
90  vpMatrix &M, vpMatrix &Mdp);
91 void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H);
92 void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H);
93 void HLM(unsigned int q_cible, unsigned int nbpt, double *xm, double *ym, double *xmi, double *ymi, vpMatrix &H);
94 
95 void HLM(unsigned int q_cible, const std::vector<double> &xm, const std::vector<double> &ym,
96  const std::vector<double> &xmi, const std::vector<double> &ymi, vpMatrix &H);
97 
98 void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &pnd, vpMatrix &pn,
99  vpMatrix &M, vpMatrix &Mdp)
100 {
101  /* Construction des matrices de changement de repere */
102  vpMatrix Md(3, 3);
103  vpMatrix Mp(3, 3);
104 
105  for (unsigned int i = 0; i < 3; i++) {
106  for (unsigned int j = 0; j < 3; j++) {
107  M[j][i] = p[pts_ref[i]][j];
108  Md[j][i] = pd[pts_ref[i]][j];
109  }
110  }
111 
112  /*calcul de la pseudo inverse */
113  Mp = M.pseudoInverse(1e-16);
114  Mdp = Md.pseudoInverse(1e-16);
115 
116  if (pts_ref[3] > 0) {
117  double lamb_des[3];
118  double lamb_cour[3];
119 
120  for (unsigned int i = 0; i < 3; i++) {
121  for (unsigned int j = 0; j < 3; j++) {
122  lamb_cour[i] = Mp[i][j] * p[pts_ref[3]][j];
123  lamb_des[i] = Mdp[i][j] * pd[pts_ref[3]][j];
124  }
125  }
126 
127  for (unsigned int i = 0; i < 3; i++) {
128  for (unsigned int j = 0; j < 3; j++) {
129  M[i][j] = M[i][j] * lamb_cour[j];
130  Md[i][j] = Md[i][j] * lamb_des[j];
131  }
132  }
133 
134  Mdp = Md.pseudoInverse(1e-16);
135  }
136 
137  /* changement de repere pour tous les points autres que
138  les trois points de reference */
139 
140  unsigned int cont_pts = 0;
141  for (unsigned int k = 0; k < nb_pts; k++) {
142  if ((pts_ref[0] != k) && (pts_ref[1] != k) && (pts_ref[2] != k)) {
143  for (unsigned int i = 0; i < 3; i++) {
144  pn[cont_pts][i] = 0.0;
145  pnd[cont_pts][i] = 0.0;
146  for (unsigned int j = 0; j < 3; j++) {
147  pn[cont_pts][i] = pn[cont_pts][i] + Mp[i][j] * p[k][j];
148  pnd[cont_pts][i] = pnd[cont_pts][i] + Mdp[i][j] * pd[k][j];
149  }
150  }
151  cont_pts = cont_pts + 1;
152  }
153  }
154 }
155 
156 /**************************************************************************
157  * NOM :
158  * Homographie_CrvMafEstHomoPointsCible2D
159  *
160  * DESCRIPTION :
161  * Calcul de l'homographie entre une image courante et une image desiree dans
162  *le cas particulier d'une cible planaire de points (cible pi). Cette
163  *procedure est appellee par "Homographie_CrvMafCalculHomographie".
164  *
165  ****************************************************************************
166  * ENTREES :
167  * int Nb_pts : nombre de points
168  * double **pd : tableau des coordonnees des points desires
169  * couble **p : tableau des coordonnees des points courants
170  *
171  * SORTIES :
172  *
173  * double **H matrice d homographie
174  *
175  ****************************************************************************
176  * AUTEUR : BOSSARD Nicolas. INSA Rennes 5eme annee.
177  *
178  * DATE DE CREATION : 02/12/98
179  *
180  * DATES DE MISE A JOUR :
181  *
182  ****************************************************************************/
183 void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H)
184 {
185  double vals_inf;
186  unsigned int contZeros, vect;
187 
189  vpMatrix M(3 * nb_pts, 9);
190  vpMatrix V(9, 9);
191  vpColVector sv(9);
192 
194  for (unsigned int j = 0; j < nb_pts; j++) {
195  M[3 * j][0] = 0;
196  M[3 * j][1] = 0;
197  M[3 * j][2] = 0;
198  M[3 * j][3] = -points_des[j][0] * points_cour[j][2];
199  M[3 * j][4] = -points_des[j][1] * points_cour[j][2];
200  M[3 * j][5] = -points_des[j][2] * points_cour[j][2];
201  M[3 * j][6] = points_des[j][0] * points_cour[j][1];
202  M[3 * j][7] = points_des[j][1] * points_cour[j][1];
203  M[3 * j][8] = points_des[j][2] * points_cour[j][1];
204 
205  M[3 * j + 1][0] = points_des[j][0] * points_cour[j][2];
206  M[3 * j + 1][1] = points_des[j][1] * points_cour[j][2];
207  M[3 * j + 1][2] = points_des[j][2] * points_cour[j][2];
208  M[3 * j + 1][3] = 0;
209  M[3 * j + 1][4] = 0;
210  M[3 * j + 1][5] = 0;
211  M[3 * j + 1][6] = -points_des[j][0] * points_cour[j][0];
212  M[3 * j + 1][7] = -points_des[j][1] * points_cour[j][0];
213  M[3 * j + 1][8] = -points_des[j][2] * points_cour[j][0];
214 
215  M[3 * j + 2][0] = -points_des[j][0] * points_cour[j][1];
216  M[3 * j + 2][1] = -points_des[j][1] * points_cour[j][1];
217  M[3 * j + 2][2] = -points_des[j][2] * points_cour[j][1];
218  M[3 * j + 2][3] = points_des[j][0] * points_cour[j][0];
219  M[3 * j + 2][4] = points_des[j][1] * points_cour[j][0];
220  M[3 * j + 2][5] = points_des[j][2] * points_cour[j][0];
221  M[3 * j + 2][6] = 0;
222  M[3 * j + 2][7] = 0;
223  M[3 * j + 2][8] = 0;
224  }
225 
227  M.svd(sv, V);
228 
229  /*****
230  La meilleure solution est le vecteur de V associe
231  a la valeur singuliere la plus petite en valeur absolu.
232  Pour cela on parcourt la matrice des valeurs singulieres
233  et on repere la plus petite valeur singuliere, on en profite
234  pour effectuer un controle sur le rang de la matrice : pas plus
235  de 2 valeurs singulieres quasi=0
236  *****/
237  vals_inf = fabs(sv[0]);
238  vect = 0;
239  contZeros = 0;
240  if (fabs(sv[0]) < eps) {
241  contZeros = contZeros + 1;
242  }
243  for (unsigned int j = 1; j < 9; j++) {
244  if (fabs(sv[j]) < vals_inf) {
245  vals_inf = fabs(sv[j]);
246  vect = j;
247  }
248  if (fabs(sv[j]) < eps) {
249  contZeros = contZeros + 1;
250  }
251  }
252 
254  if (contZeros > 2) {
255  // vpERROR_TRACE("matrix is rank deficient");
256  throw(vpMatrixException(vpMatrixException::matrixError, "matrix is rank deficient"));
257  }
258 
259  H.resize(3, 3);
261  for (unsigned int i = 0; i < 3; i++) {
262  for (unsigned int j = 0; j < 3; j++) {
263  H[i][j] = V[3 * i + j][vect];
264  }
265  }
266 }
267 
268 /**************************************************************************
269  * NOM :
270  * Homographie_CrvMafEstHomoPointsC3DEzio
271  *
272  * DESCRIPTION :
273  * Calcul de l'homographie entre une image courante et une image desiree dans
274  *le cas particulier d'une cible non planaire de points (cible pi). Cette
275  *procedure est appellee par "Homographie_CrvMafCalculHomographie".
276  *
277  *
278  ****************************************************************************
279  * ENTREES :
280  * int Nb_pts : nombre de points
281  * double **pd : tableau des coordonnees des points desires
282  * couble **p : tableau des coordonnees des points courants
283  *
284  * SORTIES :
285  *
286  * double **H matrice d'homographie
287  * double epipole[3] epipole
288  *
289  ****************************************************************************
290  **/
291 void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H)
292 {
293  unsigned int pts_ref[4]; /*** definit lesquels des points de
294  l'image sont les points de reference***/
295 
296  vpMatrix M(3, 3);
297  vpMatrix Mdp(3, 3);
298  vpMatrix c(8, 2); // matrice des coeff C
299 
300  vpColVector d(8);
301  vpMatrix cp(2, 8); // matrice pseudo-inverse de C
302 
303  vpMatrix H_int(3, 3);
304  vpMatrix pn((nb_pts - 3), 3); // points courant nouveau repere
305 
306  vpMatrix pnd((nb_pts - 3), 3); // points derives nouveau repere
307 
308  /* preparation du changement de repere */
309  /****
310  comme plan de reference on choisit pour le moment
311  arbitrairement le plan contenant les points 1,2,3 du cinq
312  ****/
313  pts_ref[0] = 0;
314  pts_ref[1] = 1;
315  pts_ref[2] = 2;
316  pts_ref[3] = 0;
317 
318  /* changement de repere pour tous les points autres que les trois points de
319  * reference */
320 
321  changeFrame(pts_ref, nb_pts, pd, p, pnd, pn, M, Mdp);
322 
323  unsigned int cont_pts = nb_pts - 3;
324 
325  if (cont_pts < 5) {
326  // vpERROR_TRACE(" not enough point to compute the homography ... ");
327  throw(vpMatrixException(vpMatrixException::matrixError, "Not enough point to compute the homography"));
328  }
329 
330  // nl = cont_pts*(cont_pts-1)*(cont_pts-2)/6 ;
331  unsigned int nc = 7;
332 
333  /* Allocation matrice CtC */
334  vpMatrix CtC(nc, nc);
335 
336  /* Initialisation matrice CtC */
337  for (unsigned int i = 0; i < nc; i++)
338  for (unsigned int j = 0; j < nc; j++)
339  CtC[i][j] = 0.0;
340 
341  /* Allocation matrice M */
342  vpColVector C(nc); // Matrice des coefficients
343 
344  /* construction de la matrice M des coefficients dans le cas general */
345  /****
346  inconnues du nouveau algorithme
347  x1 = a ; x2 = b ; x3 = c ;
348  x4 = ex ; x5 = ey ; x6 = ez ;
349  qui deviennent apres changement :
350  v1 = x1*x6 ; v2 = x1*x5 ;
351  v3 = x2*x4 ; v4 = x2*x6 ;
352  v5 = x3*x5 ; v6 = x3*x4 ;
353  ****/
354  unsigned int cont = 0;
355  for (unsigned int i = 0; i < nb_pts - 5; i++) {
356  for (unsigned int j = i + 1; j < nb_pts - 4; j++) {
357  for (unsigned int k = j + 1; k < nb_pts - 3; k++) {
358  /* coeff a^2*b */
359  C[0] = pn[i][2] * pn[j][2] * pn[k][1] * pnd[k][0] //
360  * (pnd[j][0] * pnd[i][1] - pnd[j][1] * pnd[i][0]) //
361  + pn[i][2] * pn[k][2] * pn[j][1] * pnd[j][0] //
362  * (pnd[i][0] * pnd[k][1] - pnd[i][1] * pnd[k][0]) //
363  + pn[j][2] * pn[k][2] * pn[i][1] * pnd[i][0] //
364  * (pnd[k][0] * pnd[j][1] - pnd[k][1] * pnd[j][0]); //
365  /* coeff a*b^2 */
366  C[1] = pn[i][2] * pn[j][2] * pn[k][0] * pnd[k][1] //
367  * (pnd[i][0] * pnd[j][1] - pnd[i][1] * pnd[j][0]) //
368  + pn[i][2] * pn[k][2] * pn[j][0] * pnd[j][1] //
369  * (pnd[k][0] * pnd[i][1] - pnd[k][1] * pnd[i][0]) //
370  + pn[j][2] * pn[k][2] * pn[i][0] * pnd[i][1] //
371  * (pnd[j][0] * pnd[k][1] - pnd[j][1] * pnd[k][0]); //
372  /* coeff a^2 */
373  C[2] = +pn[i][1] * pn[k][1] * pn[j][2] * pnd[j][0] //
374  * (pnd[k][2] * pnd[i][0] - pnd[k][0] * pnd[i][2]) //
375  + pn[i][1] * pn[j][1] * pn[k][2] * pnd[k][0] //
376  * (pnd[i][2] * pnd[j][0] - pnd[i][0] * pnd[j][2]) +
377  pn[j][1] * pn[k][1] * pn[i][2] * pnd[i][0] //
378  * (pnd[j][2] * pnd[k][0] - pnd[j][0] * pnd[k][2]); //
379 
380  /* coeff b^2 */
381  C[3] = pn[i][0] * pn[j][0] * pn[k][2] * pnd[k][1] //
382  * (pnd[i][2] * pnd[j][1] - pnd[i][1] * pnd[j][2]) //
383  + pn[i][0] * pn[k][0] * pn[j][2] * pnd[j][1] //
384  * (pnd[k][2] * pnd[i][1] - pnd[k][1] * pnd[i][2]) //
385  + pn[j][0] * pn[k][0] * pn[i][2] * pnd[i][1] //
386  * (pnd[j][2] * pnd[k][1] - pnd[j][1] * pnd[k][2]); //
387 
388  /* coeff a */
389  C[5] = pn[i][1] * pn[j][1] * pn[k][0] * pnd[k][2] //
390  * (pnd[i][0] * pnd[j][2] - pnd[i][2] * pnd[j][0]) //
391  + pn[i][1] * pn[k][1] * pn[j][0] * pnd[j][2] //
392  * (pnd[k][0] * pnd[i][2] - pnd[k][2] * pnd[i][0]) //
393  + pn[j][1] * pn[k][1] * pn[i][0] * pnd[i][2] //
394  * (pnd[j][0] * pnd[k][2] - pnd[j][2] * pnd[k][0]); //
395  /* coeff b */
396  C[6] = pn[i][0] * pn[j][0] * pn[k][1] * pnd[k][2] //
397  * (pnd[i][1] * pnd[j][2] - pnd[i][2] * pnd[j][1]) //
398  + pn[i][0] * pn[k][0] * pn[j][1] * pnd[j][2] //
399  * (pnd[k][1] * pnd[i][2] - pnd[k][2] * pnd[i][1]) //
400  + pn[j][0] * pn[k][0] * pn[i][1] * pnd[i][2] //
401  * (pnd[j][1] * pnd[k][2] - pnd[j][2] * pnd[k][1]); //
402  /* coeff a*b */
403  C[4] = pn[i][0] * pn[k][1] * pn[j][2] //
404  * (pnd[k][0] * pnd[j][1] * pnd[i][2] - pnd[j][0] * pnd[i][1] * pnd[k][2]) //
405  + pn[k][0] * pn[i][1] * pn[j][2] //
406  * (pnd[j][0] * pnd[k][1] * pnd[i][2] - pnd[i][0] * pnd[j][1] * pnd[k][2]) //
407  + pn[i][0] * pn[j][1] * pn[k][2] //
408  * (pnd[k][0] * pnd[i][1] * pnd[j][2] - pnd[j][0] * pnd[k][1] * pnd[i][2]) //
409  + pn[j][0] * pn[i][1] * pn[k][2] //
410  * (pnd[i][0] * pnd[k][1] * pnd[j][2] - pnd[k][0] * pnd[j][1] * pnd[i][2]) //
411  + pn[k][0] * pn[j][1] * pn[i][2] //
412  * (pnd[j][0] * pnd[i][1] * pnd[k][2] - pnd[i][0] * pnd[k][1] * pnd[j][2]) //
413  + pn[j][0] * pn[k][1] * pn[i][2] //
414  * (pnd[i][0] * pnd[j][1] * pnd[k][2] - pnd[k][0] * pnd[i][1] * pnd[j][2]); //
415 
416  cont = cont + 1;
417  /* construction de la matrice CtC */
418  for (unsigned int ii = 0; ii < nc; ii++) {
419  for (unsigned int jj = ii; jj < nc; jj++) {
420  CtC[ii][jj] = CtC[ii][jj] + C[ii] * C[jj];
421  }
422  }
423  }
424  }
425  }
426 
427  /* calcul de CtC */
428  for (unsigned int i = 0; i < nc; i++) {
429  for (unsigned int j = i + 1; j < nc; j++)
430  CtC[j][i] = CtC[i][j];
431  }
432 
433  // nl = cont ; /* nombre de lignes */
434  nc = 7; /* nombre de colonnes */
435 
436  /* Creation de matrice CtC termine */
437  /* Allocation matrice V */
438  vpMatrix V(nc, nc);
439  /*****
440  Preparation au calcul de la svd (pseudo-inverse)
441  pour obtenir une solution il faut au moins 5 equations independantes
442  donc il faut au moins la mise en correspondence de 3+5 points
443  *****/
444  vpColVector sv(nc); // Vecteur contenant les valeurs singulieres
445 
446  CtC.svd(sv, V);
447 
448  /*****
449  Il faut un controle sur le rang de la matrice !!
450  La meilleure solution est le vecteur de V associe
451  a la valeur singuliere la plus petite en valeur
452  absolu
453  *****/
454 
455  vpColVector svSorted = vpColVector::invSort(sv); // sorted singular value
456 
457  /*****
458  Parcours de la matrice ordonnee des valeurs singulieres
459  On note "cont_zeros" le nbre de valeurs quasi= a 0.
460  On note "vect" le rang de la plus petite valeur singliere
461  en valeur absolu
462  *****/
463 
464  unsigned int vect = 0;
465  int cont_zeros = 0;
466  cont = 0;
467  for (unsigned int j = 0; j < nc; j++) {
468  // if (fabs(sv[j]) == svSorted[cont]) vect = j ;
469  if (std::fabs(sv[j] - svSorted[cont]) <= std::fabs(vpMath::maximum(sv[j], svSorted[cont])))
470  vect = j;
471  if (std::fabs(sv[j] / svSorted[nc - 1]) < eps)
472  cont_zeros = cont_zeros + 1;
473  }
474 
475  if (cont_zeros > 5) {
476  // printf("erreur dans le rang de la matrice: %d \r\n ",7-cont_zeros);
477  HLM2D(nb_pts, pd, p, H);
478  } else {
479 
480  // estimation de a = 1,b,c ; je cherche le min de somme(i=1:n)
481  // (0.5*(ei)^2)
482  // e1 = V[1][.] * b - V[3][.] = 0 ;
483  // e2 = V[2][.] * c - V[3][.] = 0 ;
484  // e3 = V[2][.] * b - V[3][.] * c = 0 ;
485  // e4 = V[4][.] * b - V[5][.] = 0 ;
486  // e5 = V[4][.] * c - V[6][.] = 0 ;
487  // e6 = V[6][.] * b - V[5][.] * c = 0 ;
488  // e7 = V[7][.] * b - V[8][.] = 0 ;
489  // e8 = V[7][.] * c - V[9][.] = 0 ;
490  d[0] = V[2][vect];
491  d[1] = V[4][vect];
492  d[2] = V[1][vect];
493  d[3] = V[0][vect];
494  d[4] = V[3][vect];
495  d[5] = V[4][vect];
496  d[6] = V[0][vect];
497  d[7] = V[1][vect];
498 
499  c[0][0] = V[5][vect];
500  c[0][1] = 0.0;
501  c[1][0] = V[6][vect];
502  c[1][1] = 0.0;
503  c[2][0] = V[3][vect];
504  c[2][1] = 0.0;
505  c[3][0] = V[4][vect];
506  c[3][1] = 0.0;
507  c[4][0] = 0.0;
508  c[4][1] = V[6][vect];
509  c[5][0] = 0.0;
510  c[5][1] = V[5][vect];
511  c[6][0] = 0.0;
512  c[6][1] = V[2][vect];
513  c[7][0] = 0.0;
514  c[7][1] = V[4][vect];
515 
517  cp = c.pseudoInverse(1e-6);
518 
519  vpColVector H_nr(3), temp; // Homographie diagonale
520  // Multiplication de la matrice H_nr par le vecteur cp
521  temp = cp * d;
522 
523  H_nr[0] = temp[0];
524  H_nr[1] = temp[1];
525  H_nr[2] = 1.0;
526 
527  vpMatrix T(9, 3);
528  T = 0;
529  T[0][0] = -V[1][vect];
530  T[0][1] = V[0][vect];
531  T[1][0] = V[4][vect];
532  T[1][2] = -V[2][vect];
533  T[2][0] = -V[6][vect];
534  T[2][1] = V[2][vect];
535  T[3][0] = V[6][vect];
536  T[3][2] = -V[0][vect];
537  T[4][0] = -V[3][vect];
538  T[4][1] = V[6][vect];
539  T[5][0] = V[3][vect];
540  T[5][2] = -V[1][vect];
541  T[6][0] = -V[5][vect];
542  T[6][1] = V[4][vect];
543  T[7][0] = V[5][vect];
544  T[7][2] = -V[6][vect];
545  T[8][1] = -V[5][vect];
546  T[8][2] = V[2][vect];
547 
548  vpMatrix Hd(3, 3); // diag(gu,gv,gw)
549  for (unsigned int i = 0; i < 3; i++)
550  Hd[i][i] = H_nr[i];
551 
552  // H = M diag(gu,gv,gw) M*-1
553  H = M * Hd * Mdp;
554  }
555 }
556 
557 void HLM(unsigned int q_cible, const std::vector<double> &xm, const std::vector<double> &ym,
558  const std::vector<double> &xmi, const std::vector<double> &ymi, vpMatrix &H)
559 {
560  unsigned int nbpt = (unsigned int)xm.size();
561 
562  /****
563  on regarde si il y a au moins un point mais pour l'homographie
564  il faut au moins quatre points
565  ****/
566  vpMatrix pd(nbpt, 3);
567  vpMatrix p(nbpt, 3);
568 
569  for (unsigned int i = 0; i < nbpt; i++) {
570  /****
571  on assigne les points fournies par la structure robot
572  pour la commande globale
573  ****/
574  pd[i][0] = xmi[i];
575  pd[i][1] = ymi[i];
576  pd[i][2] = 1.0;
577  p[i][0] = xm[i];
578  p[i][1] = ym[i];
579  p[i][2] = 1.0;
580  }
581 
582  switch (q_cible) {
583  case (1):
584  case (2):
585  /* La cible est planaire de type points */
586 
587  HLM2D(nbpt, pd, p, H);
588 
589  break;
590  case (3): /* cible non planaire : chateau */
591  /* cible non planaire de type points */
592  HLM3D(nbpt, pd, p, H);
593  break;
594  } /* fin switch */
595 
596 } /* fin procedure calcul_homogaphie */
597 
598 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
599 
622 void vpHomography::HLM(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
623  const std::vector<double> &ya, bool isplanar, vpHomography &aHb)
624 {
625  unsigned int n = (unsigned int)xb.size();
626  if (yb.size() != n || xa.size() != n || ya.size() != n)
627  throw(vpException(vpException::dimensionError, "Bad dimension for HLM shomography estimation"));
628 
629  // 4 point are required
630  if (n < 4)
631  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
632 
633  // The reference plane is the plane build from the 3 first points.
634  unsigned int q_cible;
635  vpMatrix H; // matrice d'homographie en metre
636 
637  if (isplanar)
638  q_cible = 1;
639  else
640  q_cible = 3;
641 
642  ::HLM(q_cible, xa, ya, xb, yb, H);
643 
644  aHb = H;
645 }
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:1792
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
static vpColVector invSort(const vpColVector &v)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
error that can be emited by ViSP classes.
Definition: vpException.h:71
static void HLM(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, bool isplanar, vpHomography &aHb)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
error that can be emited by the vpMatrix class and its derivates
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1932