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