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