Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpHomographyExtract.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 transformation.
32 */
33
34#include <math.h>
35#include <visp3/core/vpMath.h>
36#include <visp3/vision/vpHomography.h>
37
38//#define DEBUG_Homographie 0
39
40/* ---------------------------------------------------------------------- */
41/* --- STATIC ------------------------------------------------------------ */
42/* ------------------------------------------------------------------------ */
43const double vpHomography::sing_threshold = 0.0001;
44
46{
47
48 vpColVector nd(3);
49 nd[0] = 0;
50 nd[1] = 0;
51 nd[2] = 1;
52
53 computeDisplacement(*this, aRb, atb, n);
54}
55
57 vpColVector &n)
58{
59 computeDisplacement(*this, nd, aRb, atb, n);
60}
61
62#ifndef DOXYGEN_SHOULD_SKIP_THIS
63
88{
89 /**** Declarations des variables ****/
90
91 vpMatrix aRbint(3, 3);
92 vpColVector svTemp(3), sv(3);
93 vpMatrix mX(3, 2);
94 vpColVector aTbp(3), normaleEstimee(3);
95 double distanceFictive;
96 double sinusTheta, cosinusTheta, signeSinus = 1;
97 double s, determinantU, determinantV;
98 unsigned int vOrdre[3];
99
100 // vpColVector normaleDesiree(3) ;
101 // normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
102 vpColVector normaleDesiree(nd);
103
104/**** Corps de la focntion ****/
105#ifdef DEBUG_Homographie
106 printf("debut : Homographie_EstimationDeplacementCamera\n");
107#endif
108
109 /* Allocation des matrices */
110 vpMatrix mTempU(3, 3);
111 vpMatrix mTempV(3, 3);
112 vpMatrix mU(3, 3);
113 vpMatrix mV(3, 3);
114 vpMatrix aRbp(3, 3);
115
116 vpMatrix mH(3, 3);
117 for (unsigned int i = 0; i < 3; i++)
118 for (unsigned int j = 0; j < 3; j++)
119 mH[i][j] = aHb[i][j];
120
121 /* Preparation au calcul de la SVD */
122 mTempU = mH;
123
124 /*****
125 Remarque : mTempU, svTemp et mTempV sont modifies par svd
126 Il est necessaire apres de les trier dans l'ordre decroissant
127 des valeurs singulieres
128 *****/
129 mTempU.svd(svTemp, mTempV);
130
131 /* On va mettre les valeurs singulieres en ordre decroissant : */
132
133 /* Determination de l'ordre des valeurs */
134 if (svTemp[0] >= svTemp[1]) {
135 if (svTemp[0] >= svTemp[2]) {
136 if (svTemp[1] > svTemp[2]) {
137 vOrdre[0] = 0;
138 vOrdre[1] = 1;
139 vOrdre[2] = 2;
140 } else {
141 vOrdre[0] = 0;
142 vOrdre[1] = 2;
143 vOrdre[2] = 1;
144 }
145 } else {
146 vOrdre[0] = 2;
147 vOrdre[1] = 0;
148 vOrdre[2] = 1;
149 }
150 } else {
151 if (svTemp[1] >= svTemp[2]) {
152 if (svTemp[0] > svTemp[2]) {
153 vOrdre[0] = 1;
154 vOrdre[1] = 0;
155 vOrdre[2] = 2;
156 } else {
157 vOrdre[0] = 1;
158 vOrdre[1] = 2;
159 vOrdre[2] = 0;
160 }
161 } else {
162 vOrdre[0] = 2;
163 vOrdre[1] = 1;
164 vOrdre[2] = 0;
165 }
166 }
167 /*****
168 Tri decroissant des matrices U, V, sv
169 en fonction des valeurs singulieres car
170 hypothese : sv[0]>=sv[1]>=sv[2]>=0
171 *****/
172
173 for (unsigned int i = 0; i < 3; i++) {
174 sv[i] = svTemp[vOrdre[i]];
175 for (unsigned int j = 0; j < 3; j++) {
176 mU[i][j] = mTempU[i][vOrdre[j]];
177 mV[i][j] = mTempV[i][vOrdre[j]];
178 }
179 }
180
181#ifdef DEBUG_Homographie
182 printf("U : \n");
183 std::cout << mU << std::endl;
184 printf("V : \n");
185 std::cout << mV << std::endl;
186 printf("Valeurs singulieres : ");
187 std::cout << sv.t();
188#endif
189
190 /* A verifier si necessaire!!! */
191 determinantV = mV.det();
192 determinantU = mU.det();
193
194 s = determinantU * determinantV;
195
196#ifdef DEBUG_Homographie
197 printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
198#endif
199 if (s < 0)
200 mV *= -1;
201
202 /* d' = d2 */
203 distanceFictive = sv[1];
204#ifdef DEBUG_Homographie
205 printf("d = %f\n", distanceFictive);
206#endif
207 n.resize(3);
208
209 if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) {
210 //#ifdef DEBUG_Homographie
211 // printf ("\nPure rotation\n");
212 //#endif
213 /*****
214 Cas ou le deplacement est une rotation pure
215 et la normale reste indefini
216 sv[0] = sv[1]= sv[2]
217 *****/
218 aTbp[0] = 0;
219 aTbp[1] = 0;
220 aTbp[2] = 0;
221
222 n[0] = normaleDesiree[0];
223 n[1] = normaleDesiree[1];
224 n[2] = normaleDesiree[2];
225 } else {
226#ifdef DEBUG_Homographie
227 printf("\nCas general\n");
228#endif
229 /* Cas general */
230
231 /*****
232 test pour determiner quelle est la bonne solution on teste
233 d'abord quelle solution est plus proche de la perpendiculaire
234 au plan de la cible constuction de la normale n
235 *****/
236
237 /*****
238 Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
239 dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
240 *****/
241 mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
242 mX[1][0] = 0.0;
243 mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
244
245 mX[0][1] = -mX[0][0];
246 mX[1][1] = mX[1][0];
247 mX[2][1] = mX[2][0];
248
249 /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
250 double cosinusAncien = 0.0;
251 for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */
252 for (unsigned int k = 0; k < 2; k++) { /* Pour le signe */
253
254 /* Calcul de la normale estimee : n = V.n' */
255 for (unsigned int i = 0; i < 3; i++) {
256 normaleEstimee[i] = 0.0;
257 for (unsigned int j = 0; j < 3; j++) {
258 normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
259 }
260 }
261
262 /* Calcul du cosinus de l'angle entre la normale reelle et desire */
263 double cosinusDesireeEstimee = 0.0;
264 for (unsigned int i = 0; i < 3; i++)
265 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
266
267 /*****
268 Si la solution est meilleur
269 Remarque : On ne teste pas le cas oppose (cos<0)
270 *****/
271 if (cosinusDesireeEstimee > cosinusAncien) {
272 cosinusAncien = cosinusDesireeEstimee;
273
274 /* Affectation de la normale qui est retourner */
275 for (unsigned int j = 0; j < 3; j++)
276 n[j] = normaleEstimee[j];
277
278 /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
279 aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
280 aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
281 aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
282
283 /* si c'est la deuxieme solution */
284 if (w == 1)
285 signeSinus = -1; /* car esp1*esp3 = -1 */
286 else
287 signeSinus = 1;
288 } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
289 } /* fin k */
290 } /* fin w */
291 } /* fin else */
292
293 /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
294 for (unsigned int i = 0; i < 3; i++) {
295 atb[i] = 0.0;
296 for (unsigned int j = 0; j < 3; j++) {
297 atb[i] += mU[i][j] * aTbp[j];
298 }
299 atb[i] /= distanceFictive;
300 }
301
302#ifdef DEBUG_Homographie
303 printf("t' : ");
304 std::cout << aTbp.t();
305 printf("t/d : ");
306 std::cout << atb.t();
307 printf("n : ");
308 std::cout << n.t();
309#endif
310
311 /* Calcul de la matrice de rotation R */
312
313 /*****
314 Calcul du sinus(theta) et du cosinus(theta)
315 Remarque : sinus(theta) pourra changer de signe en fonction
316 de la solution retenue (cf. ci-dessous)
317 *****/
318 sinusTheta =
319 signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
320
321 cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
322
323 /* construction de la matrice de rotation R' */
324 aRbp[0][0] = cosinusTheta;
325 aRbp[0][1] = 0;
326 aRbp[0][2] = -sinusTheta;
327 aRbp[1][0] = 0;
328 aRbp[1][1] = 1;
329 aRbp[1][2] = 0;
330 aRbp[2][0] = sinusTheta;
331 aRbp[2][1] = 0;
332 aRbp[2][2] = cosinusTheta;
333
334 /* multiplication Rint = U R' */
335 for (unsigned int i = 0; i < 3; i++) {
336 for (unsigned int j = 0; j < 3; j++) {
337 aRbint[i][j] = 0.0;
338 for (unsigned int k = 0; k < 3; k++) {
339 aRbint[i][j] += mU[i][k] * aRbp[k][j];
340 }
341 }
342 }
343
344 /* multiplication R = Rint . V^T */
345 for (unsigned int i = 0; i < 3; i++) {
346 for (unsigned int j = 0; j < 3; j++) {
347 aRb[i][j] = 0.0;
348 for (unsigned int k = 0; k < 3; k++) {
349 aRb[i][j] += aRbint[i][k] * mV[j][k];
350 }
351 }
352 }
353/*transpose_carre(aRb,3); */
354#ifdef DEBUG_Homographie
355 printf("R : %d\n", aRb.isARotationMatrix());
356 std::cout << aRb << std::endl;
357#endif
358}
359
379 vpColVector &n)
380{
381 /**** Declarations des variables ****/
382
383 vpMatrix aRbint(3, 3);
384 vpColVector svTemp(3), sv(3);
385 vpMatrix mX(3, 2);
386 vpColVector aTbp(3), normaleEstimee(3);
387 double distanceFictive;
388 double sinusTheta, cosinusTheta, signeSinus = 1;
389 double s, determinantU, determinantV;
390 unsigned int vOrdre[3];
391
392 vpColVector normaleDesiree(3);
393 normaleDesiree[0] = 0;
394 normaleDesiree[1] = 0;
395 normaleDesiree[2] = 1;
396
397/**** Corps de la focntion ****/
398#ifdef DEBUG_Homographie
399 printf("debut : Homographie_EstimationDeplacementCamera\n");
400#endif
401
402 /* Allocation des matrices */
403 vpMatrix mTempU(3, 3);
404 vpMatrix mTempV(3, 3);
405 vpMatrix mU(3, 3);
406 vpMatrix mV(3, 3);
407 vpMatrix aRbp(3, 3);
408
409 vpMatrix mH(3, 3);
410 for (unsigned int i = 0; i < 3; i++)
411 for (unsigned int j = 0; j < 3; j++)
412 mH[i][j] = aHb[i][j];
413
414 /* Preparation au calcul de la SVD */
415 mTempU = mH;
416
417 /*****
418 Remarque : mTempU, svTemp et mTempV sont modifies par svd
419 Il est necessaire apres de les trier dans l'ordre decroissant
420 des valeurs singulieres
421 *****/
422 mTempU.svd(svTemp, mTempV);
423
424 /* On va mettre les valeurs singulieres en ordre decroissant : */
425
426 /* Determination de l'ordre des valeurs */
427 if (svTemp[0] >= svTemp[1]) {
428 if (svTemp[0] >= svTemp[2]) {
429 if (svTemp[1] > svTemp[2]) {
430 vOrdre[0] = 0;
431 vOrdre[1] = 1;
432 vOrdre[2] = 2;
433 } else {
434 vOrdre[0] = 0;
435 vOrdre[1] = 2;
436 vOrdre[2] = 1;
437 }
438 } else {
439 vOrdre[0] = 2;
440 vOrdre[1] = 0;
441 vOrdre[2] = 1;
442 }
443 } else {
444 if (svTemp[1] >= svTemp[2]) {
445 if (svTemp[0] > svTemp[2]) {
446 vOrdre[0] = 1;
447 vOrdre[1] = 0;
448 vOrdre[2] = 2;
449 } else {
450 vOrdre[0] = 1;
451 vOrdre[1] = 2;
452 vOrdre[2] = 0;
453 }
454 } else {
455 vOrdre[0] = 2;
456 vOrdre[1] = 1;
457 vOrdre[2] = 0;
458 }
459 }
460 /*****
461 Tri decroissant des matrices U, V, sv
462 en fonction des valeurs singulieres car
463 hypothese : sv[0]>=sv[1]>=sv[2]>=0
464 *****/
465
466 for (unsigned int i = 0; i < 3; i++) {
467 sv[i] = svTemp[vOrdre[i]];
468 for (unsigned int j = 0; j < 3; j++) {
469 mU[i][j] = mTempU[i][vOrdre[j]];
470 mV[i][j] = mTempV[i][vOrdre[j]];
471 }
472 }
473
474#ifdef DEBUG_Homographie
475 printf("U : \n");
476 std::cout << mU << std::endl;
477 printf("V : \n");
478 std::cout << mV << std::endl;
479 printf("Valeurs singulieres : ");
480 std::cout << sv.t();
481#endif
482
483 /* A verifier si necessaire!!! */
484 determinantV = mV.det();
485 determinantU = mU.det();
486
487 s = determinantU * determinantV;
488
489#ifdef DEBUG_Homographie
490 printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
491#endif
492 if (s < 0)
493 mV *= -1;
494
495 /* d' = d2 */
496 distanceFictive = sv[1];
497#ifdef DEBUG_Homographie
498 printf("d = %f\n", distanceFictive);
499#endif
500 n.resize(3);
501
502 if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) {
503 //#ifdef DEBUG_Homographie
504 // printf ("\nPure rotation\n");
505 //#endif
506 /*****
507 Cas ou le deplacement est une rotation pure
508 et la normale reste indefini
509 sv[0] = sv[1]= sv[2]
510 *****/
511 aTbp[0] = 0;
512 aTbp[1] = 0;
513 aTbp[2] = 0;
514
515 n[0] = normaleDesiree[0];
516 n[1] = normaleDesiree[1];
517 n[2] = normaleDesiree[2];
518 } else {
519#ifdef DEBUG_Homographie
520 printf("\nCas general\n");
521#endif
522 /* Cas general */
523
524 /*****
525 test pour determiner quelle est la bonne solution on teste
526 d'abord quelle solution est plus proche de la perpendiculaire
527 au plan de la cible constuction de la normale n
528 *****/
529
530 /*****
531 Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
532 dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
533 *****/
534 mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
535 mX[1][0] = 0.0;
536 mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
537
538 mX[0][1] = -mX[0][0];
539 mX[1][1] = mX[1][0];
540 mX[2][1] = mX[2][0];
541
542 /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
543 double cosinusAncien = 0.0;
544 for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */
545 for (unsigned int k = 0; k < 2; k++) { /* Pour le signe */
546
547 /* Calcul de la normale estimee : n = V.n' */
548 for (unsigned int i = 0; i < 3; i++) {
549 normaleEstimee[i] = 0.0;
550 for (unsigned int j = 0; j < 3; j++) {
551 normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
552 }
553 }
554
555 /* Calcul du cosinus de l'angle entre la normale reelle et desire */
556 double cosinusDesireeEstimee = 0.0;
557 for (unsigned int i = 0; i < 3; i++)
558 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
559
560 /*****
561 Si la solution est meilleur
562 Remarque : On ne teste pas le cas oppose (cos<0)
563 *****/
564 if (cosinusDesireeEstimee > cosinusAncien) {
565 cosinusAncien = cosinusDesireeEstimee;
566
567 /* Affectation de la normale qui est retourner */
568 for (unsigned int j = 0; j < 3; j++)
569 n[j] = normaleEstimee[j];
570
571 /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
572 aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
573 aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
574 aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
575
576 /* si c'est la deuxieme solution */
577 if (w == 1)
578 signeSinus = -1; /* car esp1*esp3 = -1 */
579 else
580 signeSinus = 1;
581 } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
582 } /* fin k */
583 } /* fin w */
584 } /* fin else */
585
586 /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
587 for (unsigned int i = 0; i < 3; i++) {
588 atb[i] = 0.0;
589 for (unsigned int j = 0; j < 3; j++) {
590 atb[i] += mU[i][j] * aTbp[j];
591 }
592 atb[i] /= distanceFictive;
593 }
594
595#ifdef DEBUG_Homographie
596 printf("t' : ");
597 std::cout << aTbp.t();
598 printf("t/d : ");
599 std::cout << atb.t();
600 printf("n : ");
601 std::cout << n.t();
602#endif
603
604 /* Calcul de la matrice de rotation R */
605
606 /*****
607 Calcul du sinus(theta) et du cosinus(theta)
608 Remarque : sinus(theta) pourra changer de signe en fonction
609 de la solution retenue (cf. ci-dessous)
610 *****/
611 sinusTheta =
612 signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
613
614 cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
615
616 /* construction de la matrice de rotation R' */
617 aRbp[0][0] = cosinusTheta;
618 aRbp[0][1] = 0;
619 aRbp[0][2] = -sinusTheta;
620 aRbp[1][0] = 0;
621 aRbp[1][1] = 1;
622 aRbp[1][2] = 0;
623 aRbp[2][0] = sinusTheta;
624 aRbp[2][1] = 0;
625 aRbp[2][2] = cosinusTheta;
626
627 /* multiplication Rint = U R' */
628 for (unsigned int i = 0; i < 3; i++) {
629 for (unsigned int j = 0; j < 3; j++) {
630 aRbint[i][j] = 0.0;
631 for (unsigned int k = 0; k < 3; k++) {
632 aRbint[i][j] += mU[i][k] * aRbp[k][j];
633 }
634 }
635 }
636
637 /* multiplication R = Rint . V^T */
638 for (unsigned int i = 0; i < 3; i++) {
639 for (unsigned int j = 0; j < 3; j++) {
640 aRb[i][j] = 0.0;
641 for (unsigned int k = 0; k < 3; k++) {
642 aRb[i][j] += aRbint[i][k] * mV[j][k];
643 }
644 }
645 }
646/*transpose_carre(aRb,3); */
647#ifdef DEBUG_Homographie
648 printf("R : %d\n", aRb.isARotationMatrix());
649 std::cout << aRb << std::endl;
650#endif
651}
652
653void vpHomography::computeDisplacement(const vpHomography &H, double x, double y, std::list<vpRotationMatrix> &vR,
654 std::list<vpTranslationVector> &vT, std::list<vpColVector> &vN)
655{
656
657#ifdef DEBUG_Homographie
658 printf("debut : Homographie_EstimationDeplacementCamera\n");
659#endif
660
661 vR.clear();
662 vT.clear();
663 vN.clear();
664
665 /**** Declarations des variables ****/
666 int cas1 = 1, cas2 = 2, cas3 = 3, cas4 = 4;
667 int cas = 0;
668 bool norm1ok = false, norm2ok = false, norm3ok = false, norm4ok = false;
669
670 double tmp, determinantU, determinantV, s, distanceFictive;
671 vpMatrix mTempU(3, 3), mTempV(3, 3), U(3, 3), V(3, 3);
672
673 vpRotationMatrix Rprim1, R1;
674 vpRotationMatrix Rprim2, R2;
675 vpRotationMatrix Rprim3, R3;
676 vpRotationMatrix Rprim4, R4;
677 vpTranslationVector Tprim1, T1;
678 vpTranslationVector Tprim2, T2;
679 vpTranslationVector Tprim3, T3;
680 vpTranslationVector Tprim4, T4;
681 vpColVector Nprim1(3), N1(3);
682 vpColVector Nprim2(3), N2(3);
683 vpColVector Nprim3(3), N3(3);
684 vpColVector Nprim4(3), N4(3);
685
686 vpColVector svTemp(3), sv(3);
687 unsigned int vOrdre[3];
688 vpColVector vTp(3);
689
690 /* Preparation au calcul de la SVD */
691 mTempU = H.convert();
692
693 /*****
694 Remarque : mTempU, svTemp et mTempV sont modifies par svd
695 Il est necessaire apres de les trier dans l'ordre decroissant
696 des valeurs singulieres
697 *****/
698
699 // cette fonction ne renvoit pas d'erreur
700 mTempU.svd(svTemp, mTempV);
701
702 /* On va mettre les valeurs singulieres en ordre decroissant : */
703 /* Determination de l'ordre des valeurs */
704 if (svTemp[0] >= svTemp[1]) {
705 if (svTemp[0] >= svTemp[2]) {
706 if (svTemp[1] > svTemp[2]) {
707 vOrdre[0] = 0;
708 vOrdre[1] = 1;
709 vOrdre[2] = 2;
710 } else {
711 vOrdre[0] = 0;
712 vOrdre[1] = 2;
713 vOrdre[2] = 1;
714 }
715 } else {
716 vOrdre[0] = 2;
717 vOrdre[1] = 0;
718 vOrdre[2] = 1;
719 }
720 } else {
721 if (svTemp[1] >= svTemp[2]) {
722 if (svTemp[0] > svTemp[2]) {
723 vOrdre[0] = 1;
724 vOrdre[1] = 0;
725 vOrdre[2] = 2;
726 } else {
727 vOrdre[0] = 1;
728 vOrdre[1] = 2;
729 vOrdre[2] = 0;
730 }
731 } else {
732 vOrdre[0] = 2;
733 vOrdre[1] = 1;
734 vOrdre[2] = 0;
735 }
736 }
737 /*****
738 Tri decroissant des matrices U, V, sv
739 en fonction des valeurs singulieres car
740 hypothese : sv[0]>=sv[1]>=sv[2]>=0
741 *****/
742 for (unsigned int i = 0; i < 3; i++) {
743 sv[i] = svTemp[vOrdre[i]];
744 for (unsigned int j = 0; j < 3; j++) {
745 U[i][j] = mTempU[i][vOrdre[j]];
746 V[i][j] = mTempV[i][vOrdre[j]];
747 }
748 }
749
750#ifdef DEBUG_Homographie
751 printf("U : \n");
752 Affiche(U);
753 printf("V : \n");
754 affiche(V);
755 printf("Valeurs singulieres : ");
756 affiche(sv);
757#endif
758
759 // calcul du determinant de U et V
760 determinantU = U[0][0] * (U[1][1] * U[2][2] - U[1][2] * U[2][1]) + U[0][1] * (U[1][2] * U[2][0] - U[1][0] * U[2][2]) +
761 U[0][2] * (U[1][0] * U[2][1] - U[1][1] * U[2][0]);
762
763 determinantV = V[0][0] * (V[1][1] * V[2][2] - V[1][2] * V[2][1]) + V[0][1] * (V[1][2] * V[2][0] - V[1][0] * V[2][2]) +
764 V[0][2] * (V[1][0] * V[2][1] - V[1][1] * V[2][0]);
765
766 s = determinantU * determinantV;
767
768#ifdef DEBUG_Homographie
769 printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
770#endif
771
772 // deux cas sont a traiter :
773 // distance Fictive = sv[1]
774 // distance fictive = -sv[1]
775
776 // pour savoir quelle est le bon signe,
777 // on utilise le point qui appartient au plan
778 // la contrainte est :
779 // (h_31x_1 + h_32x_2 + h_33)/d > 0
780 // et d = sd'
781
782 tmp = H[2][0] * x + H[2][1] * y + H[2][2];
783
784 if ((tmp / (sv[1] * s)) > 0)
785 distanceFictive = sv[1];
786 else
787 distanceFictive = -sv[1];
788
789 // il faut ensuite considerer l'ordre de multiplicite de chaque variable
790 // 1er cas : d1<>d2<>d3
791 // 2eme cas : d1=d2 <> d3
792 // 3eme cas : d1 <>d2 =d3
793 // 4eme cas : d1 =d2=d3
794
795 if ((sv[0] - sv[1]) < sing_threshold) {
796 if ((sv[1] - sv[2]) < sing_threshold)
797 cas = cas4;
798 else
799 cas = cas2;
800 } else {
801 if ((sv[1] - sv[2]) < sing_threshold)
802 cas = cas3;
803 else
804 cas = cas1;
805 }
806
807 Nprim1 = 0.0;
808 Nprim2 = 0.0;
809 Nprim3 = 0.0;
810 Nprim4 = 0.0;
811
812 // on filtre ensuite les diff'erentes normales possibles
813 // condition : nm/d > 0
814 // avec d = sd'
815 // et n = Vn'
816
817 // les quatres cas sont : ++, +-, -+ , --
818 // dans tous les cas, Normale[1] = 0;
819 Nprim1[1] = 0.0;
820 Nprim2[1] = 0.0;
821 Nprim3[1] = 0.0;
822 Nprim4[1] = 0.0;
823
824 // on calcule les quatres cas de normale
825
826 if (cas == cas1) {
827 // quatre normales sont possibles
828 Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
829
830 Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
831
832 Nprim2[0] = Nprim1[0];
833 Nprim2[2] = -Nprim1[2];
834 Nprim3[0] = -Nprim1[0];
835 Nprim3[2] = Nprim1[2];
836 Nprim4[0] = -Nprim1[0];
837 Nprim4[2] = -Nprim1[2];
838 }
839 if (cas == cas2) {
840 // 2 normales sont possibles
841 // x3 = +-1
842 Nprim1[2] = 1;
843 Nprim2[2] = -1;
844 }
845
846 if (cas == cas3) {
847 // 2 normales sont possibles
848 // x1 = +-1
849 Nprim1[0] = 1;
850 Nprim2[0] = -1;
851 }
852 // si on est dans le cas 4,
853 // on considere que x reste indefini
854
855 // on peut maintenant filtrer les solutions avec la contrainte
856 // n^tm / d > 0
857 // attention, il faut travailler avec la bonne normale,
858 // soit Ni et non pas Nprimi
859 if (cas == cas1) {
860 N1 = V * Nprim1;
861
862 tmp = N1[0] * x + N1[1] * y + N1[2];
863 tmp /= (distanceFictive * s);
864 norm1ok = (tmp > 0);
865
866 N2 = V * Nprim2;
867 tmp = N2[0] * x + N2[1] * y + N2[2];
868 tmp /= (distanceFictive * s);
869 norm2ok = (tmp > 0);
870
871 N3 = V * Nprim3;
872 tmp = N3[0] * x + N3[1] * y + N3[2];
873 tmp /= (distanceFictive * s);
874 norm3ok = (tmp > 0);
875
876 N4 = V * Nprim4;
877 tmp = N4[0] * x + N4[1] * y + N4[2];
878 tmp /= (distanceFictive * s);
879 norm4ok = (tmp > 0);
880 }
881
882 if (cas == cas2) {
883 N1 = V * Nprim1;
884 tmp = N1[0] * x + N1[1] * y + N1[2];
885 tmp /= (distanceFictive * s);
886 norm1ok = (tmp > 0);
887
888 N2 = V * Nprim2;
889 tmp = N2[0] * x + N2[1] * y + N2[2];
890 tmp /= (distanceFictive * s);
891 norm2ok = (tmp > 0);
892 }
893
894 if (cas == cas3) {
895 N1 = V * Nprim1;
896 tmp = N1[0] * x + N1[1] * y + N1[2];
897 tmp /= (distanceFictive * s);
898 norm1ok = (tmp > 0);
899
900 N2 = V * Nprim2;
901 tmp = N2[0] * x + N2[1] * y + N2[2];
902 tmp /= (distanceFictive * s);
903 norm2ok = (tmp > 0);
904 }
905
906 // on a donc maintenant les differents cas possibles
907 // on peut ensuite remonter aux deplacements
908 // on separe encore les cas 1,2,3
909 // la, deux choix se posent suivant le signe de d
910 if (distanceFictive > 0) {
911 if (cas == cas1) {
912 if (norm1ok) {
913
914 // cos theta
915 Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
916
917 Rprim1[2][2] = Rprim1[0][0];
918
919 // sin theta
920 Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
921 ((sv[0] + sv[2]) * sv[1]);
922
923 Rprim1[0][2] = -Rprim1[2][0];
924
925 Rprim1[1][1] = 1.0;
926
927 Tprim1[0] = Nprim1[0];
928 Tprim1[1] = 0.0;
929 Tprim1[2] = -Nprim1[2];
930
931 Tprim1 *= (sv[0] - sv[2]);
932 }
933
934 if (norm2ok) {
935
936 // cos theta
937 Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
938
939 Rprim2[2][2] = Rprim2[0][0];
940
941 // sin theta
942 Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
943 ((sv[0] + sv[2]) * sv[1]);
944
945 Rprim2[0][2] = -Rprim2[2][0];
946
947 Rprim2[1][1] = 1.0;
948
949 Tprim2[0] = Nprim2[0];
950 Tprim2[1] = 0.0;
951 Tprim2[2] = -Nprim2[2];
952
953 Tprim2 *= (sv[0] - sv[2]);
954 }
955
956 if (norm3ok) {
957
958 // cos theta
959 Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
960
961 Rprim3[2][2] = Rprim3[0][0];
962
963 // sin theta
964 Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
965 ((sv[0] + sv[2]) * sv[1]);
966
967 Rprim3[0][2] = -Rprim3[2][0];
968
969 Rprim3[1][1] = 1.0;
970
971 Tprim3[0] = Nprim3[0];
972 Tprim3[1] = 0.0;
973 Tprim3[2] = -Nprim3[2];
974
975 Tprim3 *= (sv[0] - sv[2]);
976 }
977
978 if (norm4ok) {
979
980 // cos theta
981 Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
982
983 Rprim4[2][2] = Rprim4[0][0];
984
985 // sin theta
986 Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
987 ((sv[0] + sv[2]) * sv[1]);
988
989 Rprim4[0][2] = -Rprim4[2][0];
990
991 Rprim4[1][1] = 1.0;
992
993 Tprim4[0] = Nprim4[0];
994 Tprim4[1] = 0.0;
995 Tprim4[2] = -Nprim4[2];
996
997 Tprim4 *= (sv[0] - sv[2]);
998 }
999 }
1000
1001 if (cas == cas2) {
1002 // 2 normales sont potentiellement candidates
1003
1004 if (norm1ok) {
1005 Rprim1.eye();
1006
1007 Tprim1 = Nprim1[0];
1008 Tprim1 *= (sv[2] - sv[0]);
1009 }
1010
1011 if (norm2ok) {
1012 Rprim2.eye();
1013
1014 Tprim2 = Nprim2[1];
1015 Tprim2 *= (sv[2] - sv[0]);
1016 }
1017 }
1018 if (cas == cas3) {
1019 if (norm1ok) {
1020 Rprim1.eye();
1021
1022 Tprim1 = Nprim1[0];
1023 Tprim1 *= (sv[0] - sv[1]);
1024 }
1025
1026 if (norm2ok) {
1027 Rprim2.eye();
1028
1029 Tprim2 = Nprim2[1];
1030 Tprim2 *= (sv[0] - sv[1]);
1031 }
1032 }
1033 if (cas == cas4) {
1034 // on ne connait pas la normale dans ce cas la
1035 Rprim1.eye();
1036 Tprim1 = 0.0;
1037 }
1038 }
1039
1040 if (distanceFictive < 0) {
1041
1042 if (cas == cas1) {
1043 if (norm1ok) {
1044
1045 // cos theta
1046 Rprim1[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1047
1048 Rprim1[2][2] = -Rprim1[0][0];
1049
1050 // sin theta
1051 Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1052 ((sv[0] - sv[2]) * sv[1]);
1053
1054 Rprim1[0][2] = Rprim1[2][0];
1055
1056 Rprim1[1][1] = -1.0;
1057
1058 Tprim1[0] = Nprim1[0];
1059 Tprim1[1] = 0.0;
1060 Tprim1[2] = Nprim1[2];
1061
1062 Tprim1 *= (sv[0] + sv[2]);
1063 }
1064
1065 if (norm2ok) {
1066
1067 // cos theta
1068 Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1069
1070 Rprim2[2][2] = -Rprim2[0][0];
1071
1072 // sin theta
1073 Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1074 ((sv[0] - sv[2]) * sv[1]);
1075
1076 Rprim2[0][2] = Rprim2[2][0];
1077
1078 Rprim2[1][1] = -1.0;
1079
1080 Tprim2[0] = Nprim2[0];
1081 Tprim2[1] = 0.0;
1082 Tprim2[2] = Nprim2[2];
1083
1084 Tprim2 *= (sv[0] + sv[2]);
1085 }
1086
1087 if (norm3ok) {
1088
1089 // cos theta
1090 Rprim3[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1091
1092 Rprim3[2][2] = -Rprim3[0][0];
1093
1094 // sin theta
1095 Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1096 ((sv[0] - sv[2]) * sv[1]);
1097
1098 Rprim3[0][2] = Rprim3[2][0];
1099
1100 Rprim3[1][1] = -1.0;
1101
1102 Tprim3[0] = Nprim3[0];
1103 Tprim3[1] = 0.0;
1104 Tprim3[2] = Nprim3[2];
1105
1106 Tprim3 *= (sv[0] + sv[2]);
1107 }
1108
1109 if (norm4ok) {
1110 // cos theta
1111 Rprim4[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1112
1113 Rprim4[2][2] = -Rprim4[0][0];
1114
1115 // sin theta
1116 Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1117 ((sv[0] - sv[2]) * sv[1]);
1118
1119 Rprim4[0][2] = Rprim4[2][0];
1120
1121 Rprim4[1][1] = -1.0;
1122
1123 Tprim4[0] = Nprim4[0];
1124 Tprim4[1] = 0.0;
1125 Tprim4[2] = Nprim4[2];
1126
1127 Tprim4 *= (sv[0] + sv[2]);
1128 }
1129 }
1130 if (cas == cas2) {
1131 // 2 normales sont potentiellement candidates
1132
1133 if (norm1ok) {
1134 Rprim1.eye();
1135 Rprim1[0][0] = -1;
1136 Rprim1[1][1] = -1;
1137
1138 Tprim1 = Nprim1[0];
1139 Tprim1 *= (sv[2] + sv[0]);
1140 }
1141
1142 if (norm2ok) {
1143 Rprim2.eye();
1144 Rprim2[0][0] = -1;
1145 Rprim2[1][1] = -1;
1146
1147 Tprim2 = Nprim2[1];
1148 Tprim2 *= (sv[2] + sv[0]);
1149 }
1150 }
1151 if (cas == cas3) {
1152 if (norm1ok) {
1153 Rprim1.eye();
1154 Rprim1[2][2] = -1;
1155 Rprim1[1][1] = -1;
1156
1157 Tprim1 = Nprim1[0];
1158 Tprim1 *= (sv[2] + sv[0]);
1159 }
1160
1161 if (norm2ok) {
1162 Rprim2.eye();
1163 Rprim2[2][2] = -1;
1164 Rprim2[1][1] = -1;
1165
1166 Tprim2 = Nprim2[1];
1167 Tprim2 *= (sv[0] + sv[2]);
1168 }
1169 }
1170
1171 // ON NE CONSIDERE PAS LE CAS NUMERO 4
1172 }
1173 // tous les Rprim et Tprim sont calcules
1174 // on peut maintenant recuperer la
1175 // rotation, et la translation
1176 // IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
1177 if ((distanceFictive > 0) || (cas != cas4)) {
1178 // on controle juste si les normales sont ok
1179
1180 if (norm1ok) {
1181 R1 = s * U * Rprim1 * V.t();
1182 T1 = U * Tprim1;
1183 T1 /= (distanceFictive * s);
1184 N1 = V * Nprim1;
1185
1186 // je rajoute le resultat
1187 vR.push_back(R1);
1188 vT.push_back(T1);
1189 vN.push_back(N1);
1190 }
1191 if (norm2ok) {
1192 R2 = s * U * Rprim2 * V.t();
1193 T2 = U * Tprim2;
1194 T2 /= (distanceFictive * s);
1195 N2 = V * Nprim2;
1196
1197 // je rajoute le resultat
1198 vR.push_back(R2);
1199 vT.push_back(T2);
1200 vN.push_back(N2);
1201 }
1202 if (norm3ok) {
1203 R3 = s * U * Rprim3 * V.t();
1204 T3 = U * Tprim3;
1205 T3 /= (distanceFictive * s);
1206 N3 = V * Nprim3;
1207 // je rajoute le resultat
1208 vR.push_back(R3);
1209 vT.push_back(T3);
1210 vN.push_back(N3);
1211 }
1212 if (norm4ok) {
1213 R4 = s * U * Rprim4 * V.t();
1214 T4 = U * Tprim4;
1215 T4 /= (distanceFictive * s);
1216 N4 = V * Nprim4;
1217
1218 // je rajoute le resultat
1219 vR.push_back(R4);
1220 vT.push_back(T4);
1221 vN.push_back(N4);
1222 }
1223 } else {
1224 std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas "
1225 "estimable!"
1226 << std::endl;
1227 }
1228
1229 // on peut ensuite afficher les resultats...
1230 /* std::cout << "Analyse des resultats : "<< std::endl; */
1231 /* if (cas==cas1) */
1232 /* std::cout << "On est dans le cas 1" << std::endl; */
1233 /* if (cas==cas2) */
1234 /* std::cout << "On est dans le cas 2" << std::endl; */
1235 /* if (cas==cas3) */
1236 /* std::cout << "On est dans le cas 3" << std::endl; */
1237 /* if (cas==cas4) */
1238 /* std::cout << "On est dans le cas 4" << std::endl; */
1239
1240 /* if (distanceFictive < 0) */
1241 /* std::cout << "d'<0" << std::endl; */
1242 /* else */
1243 /* std::cout << "d'>0" << std::endl; */
1244
1245#ifdef DEBUG_Homographie
1246 printf("fin : Homographie_EstimationDeplacementCamera\n");
1247#endif
1248}
1249#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
Implementation of column vector and the associated operations.
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Implementation of an homography and operations on homographies.
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
vpMatrix convert() const
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void svd(vpColVector &w, vpMatrix &V)
Implementation of a rotation matrix and operations on such kind of matrices.
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix t() const
Class that consider the case of a translation vector.