ViSP  2.8.0
vpHomographyExtract.cpp
1 /****************************************************************************
2  *
3  * $Id: vpHomographyExtract.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Homography transformation.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpHomography.h>
43 #include <visp/vpMath.h>
44 #include <math.h>
45 
46 //#define DEBUG_Homographie 0
47 
48 /* ---------------------------------------------------------------------- */
49 /* --- STATIC ------------------------------------------------------------ */
50 /* ------------------------------------------------------------------------ */
51 const double vpHomography::sing_threshold = 0.0001;
52 
75 void
76  vpHomography::computeDisplacement (const vpHomography &aHb,
77  const vpColVector &nd,
78  vpRotationMatrix &aRb,
79  vpTranslationVector &atb,
80  vpColVector &n)
81 {
82  /**** Déclarations des variables ****/
83 
84  vpMatrix aRbint(3,3) ;
85  vpColVector svTemp(3), sv(3);
86  vpMatrix mX(3,2) ;
87  vpColVector aTbp(3), normaleEstimee(3);
88  double distanceFictive;
89  double sinusTheta, cosinusTheta, signeSinus= 1;
90  double cosinusDesireeEstimee, cosinusAncien;
91  double s, determinantU, determinantV;
92  unsigned int i, j, k, w;
93  unsigned int vOrdre[3];
94 
95  //vpColVector normaleDesiree(3) ;
96  //normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
97  vpColVector normaleDesiree(nd);
98 
99  /**** Corps de la focntion ****/
100 #ifdef DEBUG_Homographie
101  printf ("debut : Homographie_EstimationDeplacementCamera\n");
102 #endif
103 
104  /* Allocation des matrices */
105  vpMatrix mTempU(3,3) ;
106  vpMatrix mTempV(3,3) ;
107  vpMatrix mU(3,3) ;
108  vpMatrix mV(3,3) ;
109  vpMatrix aRbp(3,3) ;
110 
111 
112  vpMatrix mH(3,3) ;
113  for (i=0 ; i < 3 ; i++)
114  for (j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
115 
116  /* Preparation au calcul de la SVD */
117  mTempU = mH ;
118 
119  /*****
120  Remarque : mTempU, svTemp et mTempV sont modifies par svd
121  Il est necessaire apres de les trier dans l'ordre decroissant
122  des valeurs singulieres
123  *****/
124  mTempU.svd(svTemp,mTempV) ;
125 
126  /* On va mettre les valeurs singulieres en ordre decroissant : */
127 
128  /* Determination de l'ordre des valeurs */
129  if (svTemp[0] >= svTemp[1]) {
130  if (svTemp[0] >= svTemp[2]) {
131  if (svTemp[1] > svTemp[2]) {
132  vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
133  } else {
134  vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
135  }
136  } else {
137  vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
138  }
139  } else {
140  if (svTemp[1] >= svTemp[2]){
141  if (svTemp[0] > svTemp[2])
142  { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
143  else
144  { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
145  } else {
146  vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
147  }
148  }
149  /*****
150  Tri decroissant des matrices U, V, sv
151  en fonction des valeurs singulieres car
152  hypothese : sv[0]>=sv[1]>=sv[2]>=0
153  *****/
154 
155 
156  for (i = 0; i < 3; i++) {
157  sv[i] = svTemp[vOrdre[i]];
158  for (j = 0; j < 3; j++) {
159  mU[i][j] = mTempU[i][vOrdre[j]];
160  mV[i][j] = mTempV[i][vOrdre[j]];
161  }
162  }
163 
164 #ifdef DEBUG_Homographie
165  printf("U : \n") ; std::cout << mU << std::endl ;
166  printf("V : \n") ; std::cout << mV << std::endl ;
167  printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
168 #endif
169 
170  /* A verifier si necessaire!!! */
171  determinantV = mV.det();
172  determinantU = mU.det();
173 
174  s = determinantU * determinantV;
175 
176 #ifdef DEBUG_Homographie
177  printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
178 #endif
179  if (s < 0) mV *=-1 ;
180 
181 
182  /* d' = d2 */
183  distanceFictive = sv[1];
184 #ifdef DEBUG_Homographie
185  printf ("d = %f\n",distanceFictive);
186 #endif
187  n.resize(3) ;
188 
189  if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold)
190  {
191  //#ifdef DEBUG_Homographie
192  // printf ("\nPure rotation\n");
193  //#endif
194  /*****
195  Cas ou le deplacement est une rotation pure
196  et la normale reste indefini
197  sv[0] = sv[1]= sv[2]
198  *****/
199  aTbp[0] = 0;
200  aTbp[1] = 0;
201  aTbp[2] = 0;
202 
203  n[0] = normaleDesiree[0];
204  n[1] = normaleDesiree[1];
205  n[2] = normaleDesiree[2];
206  }
207  else
208  {
209 #ifdef DEBUG_Homographie
210  printf("\nCas general\n");
211 #endif
212  /* Cas general */
213 
214  /*****
215  test pour determiner quelle est la bonne solution on teste
216  d'abord quelle solution est plus proche de la perpendiculaire
217  au plan de la cible constuction de la normale n
218  *****/
219 
220  /*****
221  Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
222  dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
223  *****/
224  mX[0][0] = sqrt ((sv[0] * sv[0] - sv[1] * sv[1])
225  / (sv[0] * sv[0] - sv[2] * sv[2]));
226  mX[1][0] = 0.0;
227  mX[2][0] = sqrt ((sv[1] * sv[1] - sv[2] * sv[2])
228  / (sv[0] * sv[0] - sv[2] * sv[2]));
229 
230  mX[0][1] = -mX[0][0];
231  mX[1][1] = mX[1][0];
232  mX[2][1] = mX[2][0];
233 
234  /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
235  cosinusAncien = 0.0;
236  for (w = 0; w < 2; w++) { /* Pour les 2 cas */
237  for (k = 0; k < 2; k++) { /* Pour le signe */
238 
239  /* Calcul de la normale estimee : n = V.n' */
240  for (i = 0; i < 3; i++) {
241  normaleEstimee[i] = 0.0;
242  for (j = 0; j < 3; j++) {
243  normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
244  }
245  }
246 
247 
248  /* Calcul du cosinus de l'angle entre la normale reelle et desire */
249  cosinusDesireeEstimee = 0.0;
250  for (i = 0; i < 3; i++)
251  cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
252 
253  /*****
254  Si la solution est meilleur
255  Remarque : On ne teste pas le cas oppose (cos<0)
256  *****/
257  if (cosinusDesireeEstimee > cosinusAncien)
258  {
259  cosinusAncien = cosinusDesireeEstimee;
260 
261  /* Affectation de la normale qui est retourner */
262  for (j = 0; j < 3; j++)
263  n[j] = normaleEstimee[j];
264 
265  /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
266  aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
267  aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
268  aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
269 
270  /* si c'est la deuxieme solution */
271  if (w == 1)
272  signeSinus = -1; /* car esp1*esp3 = -1 */
273  else
274  signeSinus = 1;
275  } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
276  } /* fin k */
277  } /* fin w */
278  } /* fin else */
279 
280 
281  /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
282  for (i = 0; i < 3; i++) {
283  atb[i] = 0.0;
284  for (j = 0; j < 3; j++) {
285  atb[i] += mU[i][j] * aTbp[j];
286  }
287  atb[i] /= distanceFictive;
288  }
289 
290 
291 #ifdef DEBUG_Homographie
292  printf("t' : ") ; std::cout << aTbp.t() ;
293  printf("t/d : ") ; std::cout << atb.t() ;
294  printf("n : ") ; std::cout << n.t() ;
295 #endif
296 
297 
298  /* Calcul de la matrice de rotation R */
299 
300  /*****
301  Calcul du sinus(theta) et du cosinus(theta)
302  Remarque : sinus(theta) pourra changer de signe en fonction
303  de la solution retenue (cf. ci-dessous)
304  *****/
305  sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
306  *(sv[1]*sv[1] -sv[2]*sv[2]))
307  / ((sv[0] + sv[2]) * sv[1]);
308 
309  cosinusTheta = ( sv[1] * sv[1] + sv[0] * sv[2] ) / ((sv[0] + sv[2]) * sv[1]);
310 
311  /* construction de la matrice de rotation R' */
312  aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
313  aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
314  aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
315 
316 
317 
318  /* multiplication Rint = U R' */
319  for (i = 0; i < 3; i++) {
320  for (j = 0; j < 3; j++) {
321  aRbint[i][j] = 0.0;
322  for (k = 0; k < 3; k++) {
323  aRbint[i][j] += mU[i][k] * aRbp[k][j];
324  }
325  }
326  }
327 
328  /* multiplication R = Rint . V^T */
329  for (i = 0; i < 3; i++) {
330  for (j = 0; j < 3; j++) {
331  aRb[i][j] = 0.0;
332  for (k = 0; k < 3; k++) {
333  aRb[i][j] += aRbint[i][k] * mV[j][k];
334  }
335  }
336  }
337  /*transpose_carre(aRb,3); */
338 #ifdef DEBUG_Homographie
339  printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
340 #endif
341 
342 }
343 
365 void
366  vpHomography::computeDisplacement(const vpColVector& nd,
367  vpRotationMatrix &aRb,
368  vpTranslationVector &atb,
369  vpColVector &n)
370 {
371  computeDisplacement(*this,nd, aRb,atb,n);
372 }
391 void
392  vpHomography::computeDisplacement (const vpHomography &aHb,
393  vpRotationMatrix &aRb,
394  vpTranslationVector &atb,
395  vpColVector &n)
396 {
397  /**** Déclarations des variables ****/
398 
399  vpMatrix aRbint(3,3) ;
400  vpColVector svTemp(3), sv(3);
401  vpMatrix mX(3,2) ;
402  vpColVector aTbp(3), normaleEstimee(3);
403  double distanceFictive;
404  double sinusTheta, cosinusTheta, signeSinus= 1;
405  double cosinusDesireeEstimee, cosinusAncien;
406  double s, determinantU, determinantV;
407  unsigned int i, j, k, w;
408  unsigned int vOrdre[3];
409 
410  vpColVector normaleDesiree(3) ;
411  normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
412 
413  /**** Corps de la focntion ****/
414 #ifdef DEBUG_Homographie
415  printf ("debut : Homographie_EstimationDeplacementCamera\n");
416 #endif
417 
418  /* Allocation des matrices */
419  vpMatrix mTempU(3,3) ;
420  vpMatrix mTempV(3,3) ;
421  vpMatrix mU(3,3) ;
422  vpMatrix mV(3,3) ;
423  vpMatrix aRbp(3,3) ;
424 
425 
426  vpMatrix mH(3,3) ;
427  for (i=0 ; i < 3 ; i++)
428  for (j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
429 
430  /* Preparation au calcul de la SVD */
431  mTempU = mH ;
432 
433  /*****
434  Remarque : mTempU, svTemp et mTempV sont modifies par svd
435  Il est necessaire apres de les trier dans l'ordre decroissant
436  des valeurs singulieres
437  *****/
438  mTempU.svd(svTemp,mTempV) ;
439 
440  /* On va mettre les valeurs singulieres en ordre decroissant : */
441 
442  /* Determination de l'ordre des valeurs */
443  if (svTemp[0] >= svTemp[1]) {
444  if (svTemp[0] >= svTemp[2]) {
445  if (svTemp[1] > svTemp[2]) {
446  vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
447  } else {
448  vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
449  }
450  } else {
451  vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
452  }
453  } else {
454  if (svTemp[1] >= svTemp[2]){
455  if (svTemp[0] > svTemp[2])
456  { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
457  else
458  { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
459  } else {
460  vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
461  }
462  }
463  /*****
464  Tri decroissant des matrices U, V, sv
465  en fonction des valeurs singulieres car
466  hypothese : sv[0]>=sv[1]>=sv[2]>=0
467  *****/
468 
469 
470  for (i = 0; i < 3; i++) {
471  sv[i] = svTemp[vOrdre[i]];
472  for (j = 0; j < 3; j++) {
473  mU[i][j] = mTempU[i][vOrdre[j]];
474  mV[i][j] = mTempV[i][vOrdre[j]];
475  }
476  }
477 
478 #ifdef DEBUG_Homographie
479  printf("U : \n") ; std::cout << mU << std::endl ;
480  printf("V : \n") ; std::cout << mV << std::endl ;
481  printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
482 #endif
483 
484  /* A verifier si necessaire!!! */
485  determinantV = mV.det();
486  determinantU = mU.det();
487 
488  s = determinantU * determinantV;
489 
490 #ifdef DEBUG_Homographie
491  printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
492 #endif
493  if (s < 0) mV *=-1 ;
494 
495 
496  /* d' = d2 */
497  distanceFictive = sv[1];
498 #ifdef DEBUG_Homographie
499  printf ("d = %f\n",distanceFictive);
500 #endif
501  n.resize(3) ;
502 
503  if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold)
504  {
505  //#ifdef DEBUG_Homographie
506  // printf ("\nPure rotation\n");
507  //#endif
508  /*****
509  Cas ou le deplacement est une rotation pure
510  et la normale reste indefini
511  sv[0] = sv[1]= sv[2]
512  *****/
513  aTbp[0] = 0;
514  aTbp[1] = 0;
515  aTbp[2] = 0;
516 
517  n[0] = normaleDesiree[0];
518  n[1] = normaleDesiree[1];
519  n[2] = normaleDesiree[2];
520  }
521  else
522  {
523 #ifdef DEBUG_Homographie
524  printf("\nCas general\n");
525 #endif
526  /* Cas general */
527 
528  /*****
529  test pour determiner quelle est la bonne solution on teste
530  d'abord quelle solution est plus proche de la perpendiculaire
531  au plan de la cible constuction de la normale n
532  *****/
533 
534  /*****
535  Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
536  dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
537  *****/
538  mX[0][0] = sqrt ((sv[0] * sv[0] - sv[1] * sv[1])
539  / (sv[0] * sv[0] - sv[2] * sv[2]));
540  mX[1][0] = 0.0;
541  mX[2][0] = sqrt ((sv[1] * sv[1] - sv[2] * sv[2])
542  / (sv[0] * sv[0] - sv[2] * sv[2]));
543 
544  mX[0][1] = -mX[0][0];
545  mX[1][1] = mX[1][0];
546  mX[2][1] = mX[2][0];
547 
548  /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
549  cosinusAncien = 0.0;
550  for (w = 0; w < 2; w++) { /* Pour les 2 cas */
551  for (k = 0; k < 2; k++) { /* Pour le signe */
552 
553  /* Calcul de la normale estimee : n = V.n' */
554  for (i = 0; i < 3; i++) {
555  normaleEstimee[i] = 0.0;
556  for (j = 0; j < 3; j++) {
557  normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
558  }
559  }
560 
561 
562  /* Calcul du cosinus de l'angle entre la normale reelle et desire */
563  cosinusDesireeEstimee = 0.0;
564  for (i = 0; i < 3; i++)
565  cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
566 
567  /*****
568  Si la solution est meilleur
569  Remarque : On ne teste pas le cas oppose (cos<0)
570  *****/
571  if (cosinusDesireeEstimee > cosinusAncien)
572  {
573  cosinusAncien = cosinusDesireeEstimee;
574 
575  /* Affectation de la normale qui est retourner */
576  for (j = 0; j < 3; j++)
577  n[j] = normaleEstimee[j];
578 
579  /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
580  aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
581  aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
582  aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
583 
584  /* si c'est la deuxieme solution */
585  if (w == 1)
586  signeSinus = -1; /* car esp1*esp3 = -1 */
587  else
588  signeSinus = 1;
589  } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
590  } /* fin k */
591  } /* fin w */
592  } /* fin else */
593 
594 
595  /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
596  for (i = 0; i < 3; i++) {
597  atb[i] = 0.0;
598  for (j = 0; j < 3; j++) {
599  atb[i] += mU[i][j] * aTbp[j];
600  }
601  atb[i] /= distanceFictive;
602  }
603 
604 
605 #ifdef DEBUG_Homographie
606  printf("t' : ") ; std::cout << aTbp.t() ;
607  printf("t/d : ") ; std::cout << atb.t() ;
608  printf("n : ") ; std::cout << n.t() ;
609 #endif
610 
611 
612  /* Calcul de la matrice de rotation R */
613 
614  /*****
615  Calcul du sinus(theta) et du cosinus(theta)
616  Remarque : sinus(theta) pourra changer de signe en fonction
617  de la solution retenue (cf. ci-dessous)
618  *****/
619  sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
620  *(sv[1]*sv[1] -sv[2]*sv[2]))
621  / ((sv[0] + sv[2]) * sv[1]);
622 
623  cosinusTheta = ( sv[1] * sv[1] + sv[0] * sv[2] ) / ((sv[0] + sv[2]) * sv[1]);
624 
625  /* construction de la matrice de rotation R' */
626  aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
627  aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
628  aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
629 
630 
631 
632  /* multiplication Rint = U R' */
633  for (i = 0; i < 3; i++) {
634  for (j = 0; j < 3; j++) {
635  aRbint[i][j] = 0.0;
636  for (k = 0; k < 3; k++) {
637  aRbint[i][j] += mU[i][k] * aRbp[k][j];
638  }
639  }
640  }
641 
642  /* multiplication R = Rint . V^T */
643  for (i = 0; i < 3; i++) {
644  for (j = 0; j < 3; j++) {
645  aRb[i][j] = 0.0;
646  for (k = 0; k < 3; k++) {
647  aRb[i][j] += aRbint[i][k] * mV[j][k];
648  }
649  }
650  }
651  /*transpose_carre(aRb,3); */
652 #ifdef DEBUG_Homographie
653  printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
654 #endif
655 
656 }
657 
658 
671 void vpHomography::computeDisplacement(vpRotationMatrix &aRb,
672  vpTranslationVector &atb,
673  vpColVector &n)
674 {
675 
676 
677  vpColVector nd(3) ;
678  nd[0]=0;nd[1]=0;nd[2]=1;
679 
680  computeDisplacement(*this,aRb,atb,n);
681 
682 }
683 
684 void vpHomography::computeDisplacement(const vpMatrix &H,
685  const double x,
686  const double y,
687  std::list<vpRotationMatrix> & vR,
688  std::list<vpTranslationVector> & vT,
689  std::list<vpColVector> & vN)
690 {
691 
692 #ifdef DEBUG_Homographie
693  printf ("debut : Homographie_EstimationDeplacementCamera\n");
694 #endif
695 
696  vR.clear();
697  vT.clear();
698  vN.clear();
699 
700  /**** Déclarations des variables ****/
701  int cas1 =1, cas2=2, cas3=3, cas4=4;
702  int cas =0;
703  bool norm1ok=false, norm2ok = false,norm3ok=false,norm4ok =false;
704 
705  double tmp,determinantU,determinantV,s,distanceFictive;
706  vpMatrix mTempU(3,3),mTempV(3,3),U(3,3),V(3,3);
707 
708  vpRotationMatrix Rprim1,R1;
709  vpRotationMatrix Rprim2,R2;
710  vpRotationMatrix Rprim3,R3;
711  vpRotationMatrix Rprim4,R4;
712  vpTranslationVector Tprim1, T1;
713  vpTranslationVector Tprim2, T2;
714  vpTranslationVector Tprim3, T3;
715  vpTranslationVector Tprim4, T4;
716  vpColVector Nprim1(3), N1(3);
717  vpColVector Nprim2(3), N2(3);
718  vpColVector Nprim3(3), N3(3);
719  vpColVector Nprim4(3), N4(3);
720 
721  vpColVector svTemp(3),sv(3);
722  unsigned int vOrdre[3];
723  vpColVector vTp(3);
724 
725 
726  /* Preparation au calcul de la SVD */
727  mTempU = H;
728 
729  /*****
730  Remarque : mTempU, svTemp et mTempV sont modifies par svd
731  Il est necessaire apres de les trier dans l'ordre decroissant
732  des valeurs singulieres
733  *****/
734 
735  // cette fonction ne renvoit pas d'erreur
736  mTempU.svd(svTemp, mTempV);
737 
738  /* On va mettre les valeurs singulieres en ordre decroissant : */
739  /* Determination de l'ordre des valeurs */
740  if (svTemp[0] >= svTemp[1]) {
741  if (svTemp[0] >= svTemp[2])
742  {
743  if (svTemp[1] > svTemp[2])
744  {
745  vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
746  }
747  else
748  {
749  vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
750  }
751  }
752  else
753  {
754  vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
755  }
756  } else {
757  if (svTemp[1] >= svTemp[2]){
758  if (svTemp[0] > svTemp[2]) {
759  vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2;
760  } else {
761  vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0;
762  }
763  } else {
764  vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
765  }
766  }
767  /*****
768  Tri decroissant des matrices U, V, sv
769  en fonction des valeurs singulieres car
770  hypothese : sv[0]>=sv[1]>=sv[2]>=0
771  *****/
772  for (unsigned int i = 0; i < 3; i++) {
773  sv[i] = svTemp[vOrdre[i]];
774  for (unsigned int j = 0; j < 3; j++) {
775  U[i][j] = mTempU[i][vOrdre[j]];
776  V[i][j] = mTempV[i][vOrdre[j]];
777  }
778  }
779 
780 #ifdef DEBUG_Homographie
781  printf("U : \n") ; Affiche(U) ;
782  printf("V : \n") ; affiche(V) ;
783  printf("Valeurs singulieres : ") ; affiche(sv);
784 #endif
785 
786  // calcul du determinant de U et V
787  determinantU = U[0][0] * (U[1][1]*U[2][2] - U[1][2]*U[2][1]) +
788  U[0][1] * (U[1][2]*U[2][0] - U[1][0]*U[2][2]) +
789  U[0][2] * (U[1][0]*U[2][1] - U[1][1]*U[2][0]);
790 
791  determinantV = V[0][0] * (V[1][1]*V[2][2] - V[1][2]*V[2][1]) +
792  V[0][1] * (V[1][2]*V[2][0] - V[1][0]*V[2][2]) +
793  V[0][2] * (V[1][0]*V[2][1] - V[1][1]*V[2][0]);
794 
795  s = determinantU * determinantV;
796 
797 #ifdef DEBUG_Homographie
798  printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
799 #endif
800 
801  // deux cas sont a traiter :
802  // distance Fictive = sv[1]
803  // distance fictive = -sv[1]
804 
805  // pour savoir quelle est le bon signe,
806  // on utilise le point qui appartient au plan
807  // la contrainte est :
808  // (h_31x_1 + h_32x_2 + h_33)/d > 0
809  // et d = sd'
810 
811  tmp = H[2][0]*x + H[2][1]*y + H[2][2] ;
812 
813  if ((tmp/(sv[1] *s)) > 0)
814  distanceFictive = sv[1];
815  else
816  distanceFictive = -sv[1];
817 
818  // il faut ensuite considerer l'ordre de multiplicite de chaque variable
819  // 1er cas : d1<>d2<>d3
820  // 2eme cas : d1=d2 <> d3
821  // 3eme cas : d1 <>d2 =d3
822  // 4eme cas : d1 =d2=d3
823 
824  if ((sv[0] - sv[1]) < sing_threshold)
825  {
826  if ((sv[1] - sv[2]) < sing_threshold)
827  cas = cas4;
828  else
829  cas = cas2;
830  }
831  else
832  {
833  if ((sv[1] - sv[2]) < sing_threshold)
834  cas = cas3;
835  else
836  cas = cas1;
837  }
838 
839  Nprim1 = 0.0; Nprim2 = 0.0; Nprim3 = 0.0; Nprim4 = 0.0;
840 
841  // on filtre ensuite les diff'erentes normales possibles
842  // condition : nm/d > 0
843  // avec d = sd'
844  // et n = Vn'
845 
846  // les quatres cas sont : ++, +-, -+ , --
847  // dans tous les cas, Normale[1] = 0;
848  Nprim1[1] = 0.0;
849  Nprim2[1] = 0.0;
850  Nprim3[1] = 0.0;
851  Nprim4[1] = 0.0;
852 
853  // on calcule les quatres cas de normale
854 
855  if (cas == cas1)
856  {
857  // quatre normales sont possibles
858  Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1] )/
859  (sv[0] * sv[0] - sv[2] * sv[2] ));
860 
861  Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2] )/
862  (sv[0] * sv[0] - sv[2] * sv[2] ));
863 
864  Nprim2[0] = Nprim1[0]; Nprim2[2] = - Nprim1[2];
865  Nprim3[0] = - Nprim1[0]; Nprim3[2] = Nprim1[2];
866  Nprim4[0] = - Nprim1[0]; Nprim4[2] = - Nprim1[2];
867  }
868  if (cas == cas2)
869  {
870  // 2 normales sont possibles
871  // x3 = +-1
872  Nprim1[2] = 1;
873  Nprim2[2] = -1;
874  }
875 
876  if (cas == cas3)
877  {
878  // 2 normales sont possibles
879  // x1 = +-1
880  Nprim1[0] = 1;
881  Nprim2[0] = -1;
882  }
883  // si on est dans le cas 4,
884  // on considere que x reste indefini
885 
886  // on peut maintenant filtrer les solutions avec la contrainte
887  // n^tm / d > 0
888  // attention, il faut travailler avec la bonne normale,
889  // soit Ni et non pas Nprimi
890  if (cas == cas1)
891  {
892  N1 = V *Nprim1;
893 
894  tmp = N1[0] * x + N1[1] * y + N1[2];
895  tmp/= (distanceFictive *s);
896  norm1ok = (tmp>0);
897 
898  N2 = V *Nprim2;
899  tmp = N2[0] * x + N2[1] *y+ N2[2];
900  tmp/= (distanceFictive*s);
901  norm2ok = (tmp>0);
902 
903  N3 = V *Nprim3;
904  tmp = N3[0] * x + N3[1]*y+ N3[2];
905  tmp/= (distanceFictive*s);
906  norm3ok = (tmp>0);
907 
908  N4 = V *Nprim4;
909  tmp = N4[0] * x + N4[1] * y + N4[2];
910  tmp/= (distanceFictive*s);
911  norm4ok = (tmp>0);
912  }
913 
914  if (cas == cas2)
915  {
916  N1 = V *Nprim1;
917  tmp = N1[0] * x + N1[1] * y + N1[2];
918  tmp/= (distanceFictive*s);
919  norm1ok = (tmp>0);
920 
921  N2 = V *Nprim2;
922  tmp = N2[0] * x + N2[1] * y + N2[2];
923  tmp/= (distanceFictive*s);
924  norm2ok = (tmp>0);
925  }
926 
927  if (cas == cas3)
928  {
929  N1 = V *Nprim1;
930  tmp = N1[0] * x + N1[1] * y + N1[2];
931  tmp/= (distanceFictive*s);
932  norm1ok = (tmp>0);
933 
934  N2 = V *Nprim2;
935  tmp = N2[0] * x + N2[1] * y + N2[2];
936  tmp/= (distanceFictive*s);
937  norm2ok = (tmp>0);
938  }
939 
940  // on a donc maintenant les differents cas possibles
941  // on peut ensuite remonter aux deplacements
942  // on separe encore les cas 1,2,3
943  // la, deux choix se posent suivant le signe de d
944  if (distanceFictive>0)
945  {
946  if (cas == cas1)
947  {
948  if (norm1ok)
949  {
950 
951  //cos theta
952  Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
953  ((sv[0] + sv[2]) * sv[1]);
954 
955  Rprim1[2][2] = Rprim1[0][0];
956 
957  // sin theta
958  Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
959  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
960  / ((sv[0] + sv[2]) * sv[1]);
961 
962  Rprim1[0][2] = -Rprim1[2][0];
963 
964  Rprim1[1][1] =1.0;
965 
966  Tprim1[0] = Nprim1[0];
967  Tprim1[1] = 0.0;
968  Tprim1[2] = -Nprim1[2];
969 
970  Tprim1*=(sv[0] - sv[2]);
971 
972  }
973 
974  if (norm2ok)
975  {
976 
977  //cos theta
978  Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
979  ((sv[0] + sv[2]) * sv[1]);
980 
981  Rprim2[2][2] = Rprim2[0][0];
982 
983  // sin theta
984  Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
985  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
986  / ((sv[0] + sv[2]) * sv[1]);
987 
988  Rprim2[0][2] = -Rprim2[2][0];
989 
990  Rprim2[1][1] =1.0;
991 
992  Tprim2[0] = Nprim2[0];
993  Tprim2[1] = 0.0;
994  Tprim2[2] = -Nprim2[2];
995 
996  Tprim2*=(sv[0] - sv[2]);
997 
998  }
999 
1000  if (norm3ok)
1001  {
1002 
1003  //cos theta
1004  Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
1005  ((sv[0] + sv[2]) * sv[1]);
1006 
1007  Rprim3[2][2] = Rprim3[0][0];
1008 
1009  // sin theta
1010  Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1011  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1012  / ((sv[0] + sv[2]) * sv[1]);
1013 
1014  Rprim3[0][2] = -Rprim3[2][0];
1015 
1016  Rprim3[1][1] =1.0;
1017 
1018  Tprim3[0] = Nprim3[0];
1019  Tprim3[1] = 0.0;
1020  Tprim3[2] = -Nprim3[2];
1021 
1022  Tprim3*=(sv[0] - sv[2]);
1023 
1024  }
1025 
1026  if (norm4ok)
1027  {
1028 
1029  //cos theta
1030  Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2])/
1031  ((sv[0] + sv[2]) * sv[1]);
1032 
1033  Rprim4[2][2] = Rprim4[0][0];
1034 
1035  // sin theta
1036  Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1037  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1038  / ((sv[0] + sv[2]) * sv[1]);
1039 
1040  Rprim4[0][2] = -Rprim4[2][0];
1041 
1042  Rprim4[1][1] =1.0;
1043 
1044  Tprim4[0] = Nprim4[0];
1045  Tprim4[1] = 0.0;
1046  Tprim4[2] = -Nprim4[2];
1047 
1048  Tprim4*=(sv[0] - sv[2]);
1049 
1050  }
1051  }
1052 
1053  if (cas == cas2)
1054  {
1055  // 2 normales sont potentiellement candidates
1056 
1057  if (norm1ok)
1058  {
1059  Rprim1.setIdentity();
1060 
1061  Tprim1 = Nprim1[0];
1062  Tprim1*= (sv[2] - sv[0]);
1063  }
1064 
1065  if (norm2ok)
1066  {
1067  Rprim2.setIdentity();
1068 
1069  Tprim2 = Nprim2[1];
1070  Tprim2*= (sv[2] - sv[0]);
1071  }
1072  }
1073  if (cas == cas3)
1074  {
1075  if (norm1ok)
1076  {
1077  Rprim1.setIdentity();
1078 
1079  Tprim1 = Nprim1[0];
1080  Tprim1*= (sv[0] - sv[1]);
1081  }
1082 
1083  if (norm2ok)
1084  {
1085  Rprim2.setIdentity();
1086 
1087  Tprim2 = Nprim2[1];
1088  Tprim2*= (sv[0] - sv[1]);
1089  }
1090  }
1091  if (cas == cas4)
1092  {
1093  // on ne connait pas la normale dans ce cas la
1094  Rprim1.setIdentity();
1095  Tprim1 = 0.0;
1096  }
1097  }
1098 
1099  if (distanceFictive <0)
1100  {
1101 
1102  if (cas == cas1)
1103  {
1104  if (norm1ok)
1105  {
1106 
1107 
1108  //cos theta
1109  Rprim1[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
1110  ((sv[0] - sv[2]) * sv[1]);
1111 
1112  Rprim1[2][2] = -Rprim1[0][0];
1113 
1114  // sin theta
1115  Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1116  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1117  / ((sv[0] - sv[2]) * sv[1]);
1118 
1119  Rprim1[0][2] = Rprim1[2][0];
1120 
1121  Rprim1[1][1] = -1.0;
1122 
1123  Tprim1[0] = Nprim1[0];
1124  Tprim1[1] = 0.0;
1125  Tprim1[2] = Nprim1[2];
1126 
1127  Tprim1*=(sv[0] + sv[2]);
1128 
1129  }
1130 
1131  if (norm2ok)
1132  {
1133 
1134  //cos theta
1135  Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) /
1136  ((sv[0] - sv[2]) * sv[1]);
1137 
1138  Rprim2[2][2] = -Rprim2[0][0];
1139 
1140  // sin theta
1141  Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1142  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1143  / ((sv[0] - sv[2]) * sv[1]);
1144 
1145  Rprim2[0][2] = Rprim2[2][0];
1146 
1147  Rprim2[1][1] = - 1.0;
1148 
1149  Tprim2[0] = Nprim2[0];
1150  Tprim2[1] = 0.0;
1151  Tprim2[2] = Nprim2[2];
1152 
1153  Tprim2*=(sv[0] + sv[2]);
1154 
1155  }
1156 
1157  if (norm3ok)
1158  {
1159 
1160  //cos theta
1161  Rprim3[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
1162  ((sv[0] - sv[2]) * sv[1]);
1163 
1164  Rprim3[2][2] = -Rprim3[0][0];
1165 
1166  // sin theta
1167  Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1168  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1169  / ((sv[0] - sv[2]) * sv[1]);
1170 
1171  Rprim3[0][2] = Rprim3[2][0];
1172 
1173  Rprim3[1][1] = -1.0;
1174 
1175  Tprim3[0] = Nprim3[0];
1176  Tprim3[1] = 0.0;
1177  Tprim3[2] = Nprim3[2];
1178 
1179  Tprim3*=(sv[0] + sv[2]);
1180 
1181  }
1182 
1183  if (norm4ok)
1184  {
1185  //cos theta
1186  Rprim4[0][0] = ( sv[0] * sv[2]-vpMath::sqr(sv[1]))/((sv[0] - sv[2]) * sv[1]);
1187 
1188  Rprim4[2][2] = -Rprim4[0][0];
1189 
1190  // sin theta
1191  Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1192  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1193  / ((sv[0] - sv[2]) * sv[1]);
1194 
1195  Rprim4[0][2] = Rprim4[2][0];
1196 
1197  Rprim4[1][1] = - 1.0;
1198 
1199  Tprim4[0] = Nprim4[0];
1200  Tprim4[1] = 0.0;
1201  Tprim4[2] = Nprim4[2];
1202 
1203  Tprim4*=(sv[0] + sv[2]);
1204  }
1205  }
1206  if (cas == cas2)
1207  {
1208  // 2 normales sont potentiellement candidates
1209 
1210  if (norm1ok)
1211  {
1212  Rprim1.setIdentity();
1213  Rprim1[0][0] = -1;
1214  Rprim1[1][1] = -1;
1215 
1216  Tprim1 = Nprim1[0];
1217  Tprim1*= (sv[2] + sv[0]);
1218  }
1219 
1220  if (norm2ok)
1221  {
1222  Rprim2.setIdentity();
1223  Rprim2[0][0] = -1;
1224  Rprim2[1][1] = -1;
1225 
1226  Tprim2 = Nprim2[1];
1227  Tprim2*= (sv[2] + sv[0]);
1228  }
1229  }
1230  if (cas == cas3)
1231  {
1232  if (norm1ok)
1233  {
1234  Rprim1.setIdentity();
1235  Rprim1[2][2] = -1;
1236  Rprim1[1][1] = -1;
1237 
1238  Tprim1 = Nprim1[0];
1239  Tprim1*= (sv[2] + sv[0]);
1240  }
1241 
1242  if (norm2ok)
1243  {
1244  Rprim2.setIdentity();
1245  Rprim2[2][2] = -1;
1246  Rprim2[1][1] = -1;
1247 
1248  Tprim2 = Nprim2[1];
1249  Tprim2*= (sv[0] + sv[2]);
1250  }
1251  }
1252 
1253  // ON NE CONSIDERE PAS LE CAS NUMERO 4
1254  }
1255  // tous les Rprim et Tprim sont calcules
1256  // on peut maintenant recuperer la
1257  // rotation, et la translation
1258  // IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
1259  if ((distanceFictive>0) || (cas != cas4))
1260  {
1261  // on controle juste si les normales sont ok
1262 
1263  if (norm1ok)
1264  {
1265  R1 = s * U * Rprim1 * V.t();
1266  T1 = U * Tprim1;
1267  T1 /= (distanceFictive *s);
1268  N1 = V *Nprim1;
1269 
1270  // je rajoute le resultat
1271  vR.push_back(R1);
1272  vT.push_back(T1);
1273  vN.push_back(N1);
1274  }
1275  if (norm2ok)
1276  {
1277  R2 = s * U * Rprim2 * V.t();
1278  T2 = U * Tprim2;
1279  T2 /= (distanceFictive *s);
1280  N2 = V *Nprim2;
1281 
1282  // je rajoute le resultat
1283  vR.push_back(R2);
1284  vT.push_back(T2);
1285  vN.push_back(N2);
1286  }
1287  if (norm3ok)
1288  {
1289  R3 = s * U * Rprim3 * V.t();
1290  T3 = U * Tprim3;
1291  T3 /= (distanceFictive *s);
1292  N3 = V *Nprim3;
1293  // je rajoute le resultat
1294  vR.push_back(R3);
1295  vT.push_back(T3);
1296  vN.push_back(N3);
1297  }
1298  if (norm4ok)
1299  {
1300  R4 = s * U * Rprim4 * V.t();
1301  T4 = U * Tprim4;
1302  T4 /= (distanceFictive *s);
1303  N4 = V *Nprim4;
1304 
1305  // je rajoute le resultat
1306  vR.push_back(R4);
1307  vT.push_back(T4);
1308  vN.push_back(N4);
1309  }
1310  }
1311  else
1312  {
1313  std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas estimable!" << std::endl;
1314  }
1315 
1316  // on peut ensuite afficher les resultats...
1317  /* std::cout << "Analyse des resultats : "<< std::endl; */
1318  /* if (cas==cas1) */
1319  /* std::cout << "On est dans le cas 1" << std::endl; */
1320  /* if (cas==cas2) */
1321  /* std::cout << "On est dans le cas 2" << std::endl; */
1322  /* if (cas==cas3) */
1323  /* std::cout << "On est dans le cas 3" << std::endl; */
1324  /* if (cas==cas4) */
1325  /* std::cout << "On est dans le cas 4" << std::endl; */
1326 
1327  /* if (distanceFictive < 0) */
1328  /* std::cout << "d'<0" << std::endl; */
1329  /* else */
1330  /* std::cout << "d'>0" << std::endl; */
1331 
1332 #ifdef DEBUG_Homographie
1333  printf("fin : Homographie_EstimationDeplacementCamera\n");
1334 #endif
1335 }
1336 
1337 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1338 
1342 void vpHomography::computeDisplacement(const vpMatrix H,
1343  const double x,
1344  const double y,
1345  vpList<vpRotationMatrix> & vR,
1346  vpList<vpTranslationVector> & vT,
1347  vpList<vpColVector> & vN)
1348 {
1349 
1350 #ifdef DEBUG_Homographie
1351  printf ("debut : Homographie_EstimationDeplacementCamera\n");
1352 #endif
1353 
1354  vR.kill();
1355  vT.kill();
1356  vN.kill();
1357 
1358  /**** Déclarations des variables ****/
1359  int cas1 =1, cas2=2, cas3=3, cas4=4;
1360  int cas =0;
1361  bool norm1ok=false, norm2ok = false,norm3ok=false,norm4ok =false;
1362 
1363  double tmp,determinantU,determinantV,s,distanceFictive;
1364  vpMatrix mTempU(3,3),mTempV(3,3),U(3,3),V(3,3);
1365 
1366  vpRotationMatrix Rprim1,R1;
1367  vpRotationMatrix Rprim2,R2;
1368  vpRotationMatrix Rprim3,R3;
1369  vpRotationMatrix Rprim4,R4;
1370  vpTranslationVector Tprim1, T1;
1371  vpTranslationVector Tprim2, T2;
1372  vpTranslationVector Tprim3, T3;
1373  vpTranslationVector Tprim4, T4;
1374  vpColVector Nprim1(3), N1(3);
1375  vpColVector Nprim2(3), N2(3);
1376  vpColVector Nprim3(3), N3(3);
1377  vpColVector Nprim4(3), N4(3);
1378 
1379  vpColVector svTemp(3),sv(3);
1380  unsigned int vOrdre[3];
1381  vpColVector vTp(3);
1382 
1383 
1384  /* Preparation au calcul de la SVD */
1385  mTempU = H;
1386 
1387  /*****
1388  Remarque : mTempU, svTemp et mTempV sont modifies par svd
1389  Il est necessaire apres de les trier dans l'ordre decroissant
1390  des valeurs singulieres
1391  *****/
1392 
1393  // cette fonction ne renvoit pas d'erreur
1394  mTempU.svd(svTemp, mTempV);
1395 
1396  /* On va mettre les valeurs singulieres en ordre decroissant : */
1397  /* Determination de l'ordre des valeurs */
1398  if (svTemp[0] >= svTemp[1]) {
1399  if (svTemp[0] >= svTemp[2])
1400  {
1401  if (svTemp[1] > svTemp[2])
1402  {
1403  vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
1404  }
1405  else
1406  {
1407  vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
1408  }
1409  }
1410  else
1411  {
1412  vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
1413  }
1414  } else {
1415  if (svTemp[1] >= svTemp[2]){
1416  if (svTemp[0] > svTemp[2]) {
1417  vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2;
1418  } else {
1419  vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0;
1420  }
1421  } else {
1422  vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
1423  }
1424  }
1425  /*****
1426  Tri decroissant des matrices U, V, sv
1427  en fonction des valeurs singulieres car
1428  hypothese : sv[0]>=sv[1]>=sv[2]>=0
1429  *****/
1430  for (unsigned int i = 0; i < 3; i++) {
1431  sv[i] = svTemp[vOrdre[i]];
1432  for (unsigned int j = 0; j < 3; j++) {
1433  U[i][j] = mTempU[i][vOrdre[j]];
1434  V[i][j] = mTempV[i][vOrdre[j]];
1435  }
1436  }
1437 
1438 #ifdef DEBUG_Homographie
1439  printf("U : \n") ; Affiche(U) ;
1440  printf("V : \n") ; affiche(V) ;
1441  printf("Valeurs singulieres : ") ; affiche(sv);
1442 #endif
1443 
1444  // calcul du determinant de U et V
1445  determinantU = U[0][0] * (U[1][1]*U[2][2] - U[1][2]*U[2][1]) +
1446  U[0][1] * (U[1][2]*U[2][0] - U[1][0]*U[2][2]) +
1447  U[0][2] * (U[1][0]*U[2][1] - U[1][1]*U[2][0]);
1448 
1449  determinantV = V[0][0] * (V[1][1]*V[2][2] - V[1][2]*V[2][1]) +
1450  V[0][1] * (V[1][2]*V[2][0] - V[1][0]*V[2][2]) +
1451  V[0][2] * (V[1][0]*V[2][1] - V[1][1]*V[2][0]);
1452 
1453  s = determinantU * determinantV;
1454 
1455 #ifdef DEBUG_Homographie
1456  printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
1457 #endif
1458 
1459  // deux cas sont a traiter :
1460  // distance Fictive = sv[1]
1461  // distance fictive = -sv[1]
1462 
1463  // pour savoir quelle est le bon signe,
1464  // on utilise le point qui appartient au plan
1465  // la contrainte est :
1466  // (h_31x_1 + h_32x_2 + h_33)/d > 0
1467  // et d = sd'
1468 
1469  tmp = H[2][0]*x + H[2][1]*y + H[2][2] ;
1470 
1471  if ((tmp/(sv[1] *s)) > 0)
1472  distanceFictive = sv[1];
1473  else
1474  distanceFictive = -sv[1];
1475 
1476  // il faut ensuite considerer l'ordre de multiplicite de chaque variable
1477  // 1er cas : d1<>d2<>d3
1478  // 2eme cas : d1=d2 <> d3
1479  // 3eme cas : d1 <>d2 =d3
1480  // 4eme cas : d1 =d2=d3
1481 
1482  if ((sv[0] - sv[1]) < sing_threshold)
1483  {
1484  if ((sv[1] - sv[2]) < sing_threshold)
1485  cas = cas4;
1486  else
1487  cas = cas2;
1488  }
1489  else
1490  {
1491  if ((sv[1] - sv[2]) < sing_threshold)
1492  cas = cas3;
1493  else
1494  cas = cas1;
1495  }
1496 
1497  Nprim1 = 0.0; Nprim2 = 0.0; Nprim3 = 0.0; Nprim4 = 0.0;
1498 
1499  // on filtre ensuite les diff'erentes normales possibles
1500  // condition : nm/d > 0
1501  // avec d = sd'
1502  // et n = Vn'
1503 
1504  // les quatres cas sont : ++, +-, -+ , --
1505  // dans tous les cas, Normale[1] = 0;
1506  Nprim1[1] = 0.0;
1507  Nprim2[1] = 0.0;
1508  Nprim3[1] = 0.0;
1509  Nprim4[1] = 0.0;
1510 
1511  // on calcule les quatres cas de normale
1512 
1513  if (cas == cas1)
1514  {
1515  // quatre normales sont possibles
1516  Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1] )/
1517  (sv[0] * sv[0] - sv[2] * sv[2] ));
1518 
1519  Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2] )/
1520  (sv[0] * sv[0] - sv[2] * sv[2] ));
1521 
1522  Nprim2[0] = Nprim1[0]; Nprim2[2] = - Nprim1[2];
1523  Nprim3[0] = - Nprim1[0]; Nprim3[2] = Nprim1[2];
1524  Nprim4[0] = - Nprim1[0]; Nprim4[2] = - Nprim1[2];
1525  }
1526  if (cas == cas2)
1527  {
1528  // 2 normales sont possibles
1529  // x3 = +-1
1530  Nprim1[2] = 1;
1531  Nprim2[2] = -1;
1532  }
1533 
1534  if (cas == cas3)
1535  {
1536  // 2 normales sont possibles
1537  // x1 = +-1
1538  Nprim1[0] = 1;
1539  Nprim2[0] = -1;
1540  }
1541  // si on est dans le cas 4,
1542  // on considere que x reste indefini
1543 
1544  // on peut maintenant filtrer les solutions avec la contrainte
1545  // n^tm / d > 0
1546  // attention, il faut travailler avec la bonne normale,
1547  // soit Ni et non pas Nprimi
1548  if (cas == cas1)
1549  {
1550  N1 = V *Nprim1;
1551 
1552  tmp = N1[0] * x + N1[1] * y + N1[2];
1553  tmp/= (distanceFictive *s);
1554  norm1ok = (tmp>0);
1555 
1556  N2 = V *Nprim2;
1557  tmp = N2[0] * x + N2[1] *y+ N2[2];
1558  tmp/= (distanceFictive*s);
1559  norm2ok = (tmp>0);
1560 
1561  N3 = V *Nprim3;
1562  tmp = N3[0] * x + N3[1]*y+ N3[2];
1563  tmp/= (distanceFictive*s);
1564  norm3ok = (tmp>0);
1565 
1566  N4 = V *Nprim4;
1567  tmp = N4[0] * x + N4[1] * y + N4[2];
1568  tmp/= (distanceFictive*s);
1569  norm4ok = (tmp>0);
1570  }
1571 
1572  if (cas == cas2)
1573  {
1574  N1 = V *Nprim1;
1575  tmp = N1[0] * x + N1[1] * y + N1[2];
1576  tmp/= (distanceFictive*s);
1577  norm1ok = (tmp>0);
1578 
1579  N2 = V *Nprim2;
1580  tmp = N2[0] * x + N2[1] * y + N2[2];
1581  tmp/= (distanceFictive*s);
1582  norm2ok = (tmp>0);
1583  }
1584 
1585  if (cas == cas3)
1586  {
1587  N1 = V *Nprim1;
1588  tmp = N1[0] * x + N1[1] * y + N1[2];
1589  tmp/= (distanceFictive*s);
1590  norm1ok = (tmp>0);
1591 
1592  N2 = V *Nprim2;
1593  tmp = N2[0] * x + N2[1] * y + N2[2];
1594  tmp/= (distanceFictive*s);
1595  norm2ok = (tmp>0);
1596  }
1597 
1598  // on a donc maintenant les differents cas possibles
1599  // on peut ensuite remonter aux deplacements
1600  // on separe encore les cas 1,2,3
1601  // la, deux choix se posent suivant le signe de d
1602  if (distanceFictive>0)
1603  {
1604  if (cas == cas1)
1605  {
1606  if (norm1ok)
1607  {
1608 
1609  //cos theta
1610  Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
1611  ((sv[0] + sv[2]) * sv[1]);
1612 
1613  Rprim1[2][2] = Rprim1[0][0];
1614 
1615  // sin theta
1616  Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1617  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1618  / ((sv[0] + sv[2]) * sv[1]);
1619 
1620  Rprim1[0][2] = -Rprim1[2][0];
1621 
1622  Rprim1[1][1] =1.0;
1623 
1624  Tprim1[0] = Nprim1[0];
1625  Tprim1[1] = 0.0;
1626  Tprim1[2] = -Nprim1[2];
1627 
1628  Tprim1*=(sv[0] - sv[2]);
1629 
1630  }
1631 
1632  if (norm2ok)
1633  {
1634 
1635  //cos theta
1636  Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
1637  ((sv[0] + sv[2]) * sv[1]);
1638 
1639  Rprim2[2][2] = Rprim2[0][0];
1640 
1641  // sin theta
1642  Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1643  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1644  / ((sv[0] + sv[2]) * sv[1]);
1645 
1646  Rprim2[0][2] = -Rprim2[2][0];
1647 
1648  Rprim2[1][1] =1.0;
1649 
1650  Tprim2[0] = Nprim2[0];
1651  Tprim2[1] = 0.0;
1652  Tprim2[2] = -Nprim2[2];
1653 
1654  Tprim2*=(sv[0] - sv[2]);
1655 
1656  }
1657 
1658  if (norm3ok)
1659  {
1660 
1661  //cos theta
1662  Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
1663  ((sv[0] + sv[2]) * sv[1]);
1664 
1665  Rprim3[2][2] = Rprim3[0][0];
1666 
1667  // sin theta
1668  Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1669  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1670  / ((sv[0] + sv[2]) * sv[1]);
1671 
1672  Rprim3[0][2] = -Rprim3[2][0];
1673 
1674  Rprim3[1][1] =1.0;
1675 
1676  Tprim3[0] = Nprim3[0];
1677  Tprim3[1] = 0.0;
1678  Tprim3[2] = -Nprim3[2];
1679 
1680  Tprim3*=(sv[0] - sv[2]);
1681 
1682  }
1683 
1684  if (norm4ok)
1685  {
1686 
1687  //cos theta
1688  Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2])/
1689  ((sv[0] + sv[2]) * sv[1]);
1690 
1691  Rprim4[2][2] = Rprim4[0][0];
1692 
1693  // sin theta
1694  Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1695  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1696  / ((sv[0] + sv[2]) * sv[1]);
1697 
1698  Rprim4[0][2] = -Rprim4[2][0];
1699 
1700  Rprim4[1][1] =1.0;
1701 
1702  Tprim4[0] = Nprim4[0];
1703  Tprim4[1] = 0.0;
1704  Tprim4[2] = -Nprim4[2];
1705 
1706  Tprim4*=(sv[0] - sv[2]);
1707 
1708  }
1709  }
1710 
1711  if (cas == cas2)
1712  {
1713  // 2 normales sont potentiellement candidates
1714 
1715  if (norm1ok)
1716  {
1717  Rprim1.setIdentity();
1718 
1719  Tprim1 = Nprim1[0];
1720  Tprim1*= (sv[2] - sv[0]);
1721  }
1722 
1723  if (norm2ok)
1724  {
1725  Rprim2.setIdentity();
1726 
1727  Tprim2 = Nprim2[1];
1728  Tprim2*= (sv[2] - sv[0]);
1729  }
1730  }
1731  if (cas == cas3)
1732  {
1733  if (norm1ok)
1734  {
1735  Rprim1.setIdentity();
1736 
1737  Tprim1 = Nprim1[0];
1738  Tprim1*= (sv[0] - sv[1]);
1739  }
1740 
1741  if (norm2ok)
1742  {
1743  Rprim2.setIdentity();
1744 
1745  Tprim2 = Nprim2[1];
1746  Tprim2*= (sv[0] - sv[1]);
1747  }
1748  }
1749  if (cas == cas4)
1750  {
1751  // on ne connait pas la normale dans ce cas la
1752  Rprim1.setIdentity();
1753  Tprim1 = 0.0;
1754  }
1755  }
1756 
1757  if (distanceFictive <0)
1758  {
1759 
1760  if (cas == cas1)
1761  {
1762  if (norm1ok)
1763  {
1764 
1765 
1766  //cos theta
1767  Rprim1[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
1768  ((sv[0] - sv[2]) * sv[1]);
1769 
1770  Rprim1[2][2] = -Rprim1[0][0];
1771 
1772  // sin theta
1773  Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1774  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1775  / ((sv[0] - sv[2]) * sv[1]);
1776 
1777  Rprim1[0][2] = Rprim1[2][0];
1778 
1779  Rprim1[1][1] = -1.0;
1780 
1781  Tprim1[0] = Nprim1[0];
1782  Tprim1[1] = 0.0;
1783  Tprim1[2] = Nprim1[2];
1784 
1785  Tprim1*=(sv[0] + sv[2]);
1786 
1787  }
1788 
1789  if (norm2ok)
1790  {
1791 
1792  //cos theta
1793  Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) /
1794  ((sv[0] - sv[2]) * sv[1]);
1795 
1796  Rprim2[2][2] = -Rprim2[0][0];
1797 
1798  // sin theta
1799  Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1800  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1801  / ((sv[0] - sv[2]) * sv[1]);
1802 
1803  Rprim2[0][2] = Rprim2[2][0];
1804 
1805  Rprim2[1][1] = - 1.0;
1806 
1807  Tprim2[0] = Nprim2[0];
1808  Tprim2[1] = 0.0;
1809  Tprim2[2] = Nprim2[2];
1810 
1811  Tprim2*=(sv[0] + sv[2]);
1812 
1813  }
1814 
1815  if (norm3ok)
1816  {
1817 
1818  //cos theta
1819  Rprim3[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
1820  ((sv[0] - sv[2]) * sv[1]);
1821 
1822  Rprim3[2][2] = -Rprim3[0][0];
1823 
1824  // sin theta
1825  Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1826  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1827  / ((sv[0] - sv[2]) * sv[1]);
1828 
1829  Rprim3[0][2] = Rprim3[2][0];
1830 
1831  Rprim3[1][1] = -1.0;
1832 
1833  Tprim3[0] = Nprim3[0];
1834  Tprim3[1] = 0.0;
1835  Tprim3[2] = Nprim3[2];
1836 
1837  Tprim3*=(sv[0] + sv[2]);
1838 
1839  }
1840 
1841  if (norm4ok)
1842  {
1843  //cos theta
1844  Rprim4[0][0] = ( sv[0] * sv[2]-vpMath::sqr(sv[1]))/((sv[0] - sv[2]) * sv[1]);
1845 
1846  Rprim4[2][2] = -Rprim4[0][0];
1847 
1848  // sin theta
1849  Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
1850  (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
1851  / ((sv[0] - sv[2]) * sv[1]);
1852 
1853  Rprim4[0][2] = Rprim4[2][0];
1854 
1855  Rprim4[1][1] = - 1.0;
1856 
1857  Tprim4[0] = Nprim4[0];
1858  Tprim4[1] = 0.0;
1859  Tprim4[2] = Nprim4[2];
1860 
1861  Tprim4*=(sv[0] + sv[2]);
1862  }
1863  }
1864  if (cas == cas2)
1865  {
1866  // 2 normales sont potentiellement candidates
1867 
1868  if (norm1ok)
1869  {
1870  Rprim1.setIdentity();
1871  Rprim1[0][0] = -1;
1872  Rprim1[1][1] = -1;
1873 
1874  Tprim1 = Nprim1[0];
1875  Tprim1*= (sv[2] + sv[0]);
1876  }
1877 
1878  if (norm2ok)
1879  {
1880  Rprim2.setIdentity();
1881  Rprim2[0][0] = -1;
1882  Rprim2[1][1] = -1;
1883 
1884  Tprim2 = Nprim2[1];
1885  Tprim2*= (sv[2] + sv[0]);
1886  }
1887  }
1888  if (cas == cas3)
1889  {
1890  if (norm1ok)
1891  {
1892  Rprim1.setIdentity();
1893  Rprim1[2][2] = -1;
1894  Rprim1[1][1] = -1;
1895 
1896  Tprim1 = Nprim1[0];
1897  Tprim1*= (sv[2] + sv[0]);
1898  }
1899 
1900  if (norm2ok)
1901  {
1902  Rprim2.setIdentity();
1903  Rprim2[2][2] = -1;
1904  Rprim2[1][1] = -1;
1905 
1906  Tprim2 = Nprim2[1];
1907  Tprim2*= (sv[0] + sv[2]);
1908  }
1909  }
1910 
1911  // ON NE CONSIDERE PAS LE CAS NUMERO 4
1912  }
1913  // tous les Rprim et Tprim sont calcules
1914  // on peut maintenant recuperer la
1915  // rotation, et la translation
1916  // IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
1917  if ((distanceFictive>0) || (cas != cas4))
1918  {
1919  // on controle juste si les normales sont ok
1920 
1921  if (norm1ok)
1922  {
1923  R1 = s * U * Rprim1 * V.t();
1924  T1 = U * Tprim1;
1925  T1 /= (distanceFictive *s);
1926  N1 = V *Nprim1;
1927 
1928  // je rajoute le resultat
1929  vR.addRight(R1);
1930  vT.addRight(T1);
1931  vN.addRight(N1);
1932  }
1933  if (norm2ok)
1934  {
1935  R2 = s * U * Rprim2 * V.t();
1936  T2 = U * Tprim2;
1937  T2 /= (distanceFictive *s);
1938  N2 = V *Nprim2;
1939 
1940  // je rajoute le resultat
1941  vR.addRight(R2);
1942  vT.addRight(T2);
1943  vN.addRight(N2);
1944  }
1945  if (norm3ok)
1946  {
1947  R3 = s * U * Rprim3 * V.t();
1948  T3 = U * Tprim3;
1949  T3 /= (distanceFictive *s);
1950  N3 = V *Nprim3;
1951  // je rajoute le resultat
1952  vR.addRight(R3);
1953  vT.addRight(T3);
1954  vN.addRight(N3);
1955  }
1956  if (norm4ok)
1957  {
1958  R4 = s * U * Rprim4 * V.t();
1959  T4 = U * Tprim4;
1960  T4 /= (distanceFictive *s);
1961  N4 = V *Nprim4;
1962 
1963  // je rajoute le resultat
1964  vR.addRight(R4);
1965  vT.addRight(T4);
1966  vN.addRight(N4);
1967  }
1968  }
1969  else
1970  {
1971  std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas estimable!" << std::endl;
1972  }
1973 
1974  // on peut ensuite afficher les resultats...
1975  /* std::cout << "Analyse des resultats : "<< std::endl; */
1976  /* if (cas==cas1) */
1977  /* std::cout << "On est dans le cas 1" << std::endl; */
1978  /* if (cas==cas2) */
1979  /* std::cout << "On est dans le cas 2" << std::endl; */
1980  /* if (cas==cas3) */
1981  /* std::cout << "On est dans le cas 3" << std::endl; */
1982  /* if (cas==cas4) */
1983  /* std::cout << "On est dans le cas 4" << std::endl; */
1984 
1985  /* if (distanceFictive < 0) */
1986  /* std::cout << "d'<0" << std::endl; */
1987  /* else */
1988  /* std::cout << "d'>0" << std::endl; */
1989 
1990 #ifdef DEBUG_Homographie
1991  printf("fin : Homographie_EstimationDeplacementCamera\n");
1992 #endif
1993 }
1994 
1995 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS
vpMatrix
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpList
Provide simple list management.
Definition: vpList.h:112
vpRotationMatrix::t
vpRotationMatrix t() const
transpose
Definition: vpRotationMatrix.cpp:421
vpList::kill
void kill()
Destroy the list.
Definition: vpList.h:694
vpRotationMatrix::setIdentity
void setIdentity()
Basic initialisation (identity)
Definition: vpRotationMatrix.cpp:100
vpHomography::computeDisplacement
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Definition: vpHomographyExtract.cpp:671
vpRotationMatrix::isARotationMatrix
bool isARotationMatrix() const
test if the matrix is an rotation matrix
Definition: vpRotationMatrix.cpp:314
vpRotationMatrix
The vpRotationMatrix considers the particular case of a rotation matrix.
Definition: vpRotationMatrix.h:72
vpHomography
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:173
vpMatrix::svd
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1702
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:106
vpColVector::t
vpRowVector t() const
transpose of Vector
Definition: vpColVector.cpp:307
vpList::addRight
void addRight(const type &el)
add a new element in the list, at the right of the current one
Definition: vpList.h:480
vpMatrix::det
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:3413
vpColVector
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:95
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94