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