Visual Servoing Platform version 3.5.0
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
58const 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
89void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &pnd, vpMatrix &pn,
90 vpMatrix &M, vpMatrix &Mdp);
91void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H);
92void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H);
93void HLM(unsigned int q_cible, unsigned int nbpt, double *xm, double *ym, double *xmi, double *ymi, vpMatrix &H);
94
95void 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
98void 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 ****************************************************************************/
183void 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 = sv[0];
238 vect = 0;
239 contZeros = 0;
240 if (sv[0] < eps) {
241 contZeros = contZeros + 1;
242 }
243 for (unsigned int j = 1; j < 9; j++) {
244 if (sv[j] < vals_inf) {
245 vals_inf = sv[j];
246 vect = j;
247 }
248 if (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 **/
291void 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
557void 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
622void 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 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
static vpColVector invSort(const vpColVector &v)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
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:145
error that can be emited by the vpMatrix class and its derivates
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241