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