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