Visual Servoing Platform  version 3.5.1 under development (2022-12-06)
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, double x, double y, std::list<vpRotationMatrix> &vR,
690  std::list<vpTranslationVector> &vT, std::list<vpColVector> &vN)
691 {
692 
693 #ifdef DEBUG_Homographie
694  printf("debut : Homographie_EstimationDeplacementCamera\n");
695 #endif
696 
697  vR.clear();
698  vT.clear();
699  vN.clear();
700 
701  /**** Declarations des variables ****/
702  int cas1 = 1, cas2 = 2, cas3 = 3, cas4 = 4;
703  int cas = 0;
704  bool norm1ok = false, norm2ok = false, norm3ok = false, norm4ok = false;
705 
706  double tmp, determinantU, determinantV, s, distanceFictive;
707  vpMatrix mTempU(3, 3), mTempV(3, 3), U(3, 3), V(3, 3);
708 
709  vpRotationMatrix Rprim1, R1;
710  vpRotationMatrix Rprim2, R2;
711  vpRotationMatrix Rprim3, R3;
712  vpRotationMatrix Rprim4, R4;
713  vpTranslationVector Tprim1, T1;
714  vpTranslationVector Tprim2, T2;
715  vpTranslationVector Tprim3, T3;
716  vpTranslationVector Tprim4, T4;
717  vpColVector Nprim1(3), N1(3);
718  vpColVector Nprim2(3), N2(3);
719  vpColVector Nprim3(3), N3(3);
720  vpColVector Nprim4(3), N4(3);
721 
722  vpColVector svTemp(3), sv(3);
723  unsigned int vOrdre[3];
724  vpColVector vTp(3);
725 
726  /* Preparation au calcul de la SVD */
727  mTempU = H.convert();
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  if (svTemp[1] > svTemp[2]) {
743  vOrdre[0] = 0;
744  vOrdre[1] = 1;
745  vOrdre[2] = 2;
746  } else {
747  vOrdre[0] = 0;
748  vOrdre[1] = 2;
749  vOrdre[2] = 1;
750  }
751  } else {
752  vOrdre[0] = 2;
753  vOrdre[1] = 0;
754  vOrdre[2] = 1;
755  }
756  } else {
757  if (svTemp[1] >= svTemp[2]) {
758  if (svTemp[0] > svTemp[2]) {
759  vOrdre[0] = 1;
760  vOrdre[1] = 0;
761  vOrdre[2] = 2;
762  } else {
763  vOrdre[0] = 1;
764  vOrdre[1] = 2;
765  vOrdre[2] = 0;
766  }
767  } else {
768  vOrdre[0] = 2;
769  vOrdre[1] = 1;
770  vOrdre[2] = 0;
771  }
772  }
773  /*****
774  Tri decroissant des matrices U, V, sv
775  en fonction des valeurs singulieres car
776  hypothese : sv[0]>=sv[1]>=sv[2]>=0
777  *****/
778  for (unsigned int i = 0; i < 3; i++) {
779  sv[i] = svTemp[vOrdre[i]];
780  for (unsigned int j = 0; j < 3; j++) {
781  U[i][j] = mTempU[i][vOrdre[j]];
782  V[i][j] = mTempV[i][vOrdre[j]];
783  }
784  }
785 
786 #ifdef DEBUG_Homographie
787  printf("U : \n");
788  Affiche(U);
789  printf("V : \n");
790  affiche(V);
791  printf("Valeurs singulieres : ");
792  affiche(sv);
793 #endif
794 
795  // calcul du determinant de U et V
796  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]) +
797  U[0][2] * (U[1][0] * U[2][1] - U[1][1] * U[2][0]);
798 
799  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]) +
800  V[0][2] * (V[1][0] * V[2][1] - V[1][1] * V[2][0]);
801 
802  s = determinantU * determinantV;
803 
804 #ifdef DEBUG_Homographie
805  printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
806 #endif
807 
808  // deux cas sont a traiter :
809  // distance Fictive = sv[1]
810  // distance fictive = -sv[1]
811 
812  // pour savoir quelle est le bon signe,
813  // on utilise le point qui appartient au plan
814  // la contrainte est :
815  // (h_31x_1 + h_32x_2 + h_33)/d > 0
816  // et d = sd'
817 
818  tmp = H[2][0] * x + H[2][1] * y + H[2][2];
819 
820  if ((tmp / (sv[1] * s)) > 0)
821  distanceFictive = sv[1];
822  else
823  distanceFictive = -sv[1];
824 
825  // il faut ensuite considerer l'ordre de multiplicite de chaque variable
826  // 1er cas : d1<>d2<>d3
827  // 2eme cas : d1=d2 <> d3
828  // 3eme cas : d1 <>d2 =d3
829  // 4eme cas : d1 =d2=d3
830 
831  if ((sv[0] - sv[1]) < sing_threshold) {
832  if ((sv[1] - sv[2]) < sing_threshold)
833  cas = cas4;
834  else
835  cas = cas2;
836  } else {
837  if ((sv[1] - sv[2]) < sing_threshold)
838  cas = cas3;
839  else
840  cas = cas1;
841  }
842 
843  Nprim1 = 0.0;
844  Nprim2 = 0.0;
845  Nprim3 = 0.0;
846  Nprim4 = 0.0;
847 
848  // on filtre ensuite les diff'erentes normales possibles
849  // condition : nm/d > 0
850  // avec d = sd'
851  // et n = Vn'
852 
853  // les quatres cas sont : ++, +-, -+ , --
854  // dans tous les cas, Normale[1] = 0;
855  Nprim1[1] = 0.0;
856  Nprim2[1] = 0.0;
857  Nprim3[1] = 0.0;
858  Nprim4[1] = 0.0;
859 
860  // on calcule les quatres cas de normale
861 
862  if (cas == cas1) {
863  // quatre normales sont possibles
864  Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
865 
866  Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
867 
868  Nprim2[0] = Nprim1[0];
869  Nprim2[2] = -Nprim1[2];
870  Nprim3[0] = -Nprim1[0];
871  Nprim3[2] = Nprim1[2];
872  Nprim4[0] = -Nprim1[0];
873  Nprim4[2] = -Nprim1[2];
874  }
875  if (cas == cas2) {
876  // 2 normales sont possibles
877  // x3 = +-1
878  Nprim1[2] = 1;
879  Nprim2[2] = -1;
880  }
881 
882  if (cas == cas3) {
883  // 2 normales sont possibles
884  // x1 = +-1
885  Nprim1[0] = 1;
886  Nprim2[0] = -1;
887  }
888  // si on est dans le cas 4,
889  // on considere que x reste indefini
890 
891  // on peut maintenant filtrer les solutions avec la contrainte
892  // n^tm / d > 0
893  // attention, il faut travailler avec la bonne normale,
894  // soit Ni et non pas Nprimi
895  if (cas == cas1) {
896  N1 = V * Nprim1;
897 
898  tmp = N1[0] * x + N1[1] * y + N1[2];
899  tmp /= (distanceFictive * s);
900  norm1ok = (tmp > 0);
901 
902  N2 = V * Nprim2;
903  tmp = N2[0] * x + N2[1] * y + N2[2];
904  tmp /= (distanceFictive * s);
905  norm2ok = (tmp > 0);
906 
907  N3 = V * Nprim3;
908  tmp = N3[0] * x + N3[1] * y + N3[2];
909  tmp /= (distanceFictive * s);
910  norm3ok = (tmp > 0);
911 
912  N4 = V * Nprim4;
913  tmp = N4[0] * x + N4[1] * y + N4[2];
914  tmp /= (distanceFictive * s);
915  norm4ok = (tmp > 0);
916  }
917 
918  if (cas == cas2) {
919  N1 = V * Nprim1;
920  tmp = N1[0] * x + N1[1] * y + N1[2];
921  tmp /= (distanceFictive * s);
922  norm1ok = (tmp > 0);
923 
924  N2 = V * Nprim2;
925  tmp = N2[0] * x + N2[1] * y + N2[2];
926  tmp /= (distanceFictive * s);
927  norm2ok = (tmp > 0);
928  }
929 
930  if (cas == cas3) {
931  N1 = V * Nprim1;
932  tmp = N1[0] * x + N1[1] * y + N1[2];
933  tmp /= (distanceFictive * s);
934  norm1ok = (tmp > 0);
935 
936  N2 = V * Nprim2;
937  tmp = N2[0] * x + N2[1] * y + N2[2];
938  tmp /= (distanceFictive * s);
939  norm2ok = (tmp > 0);
940  }
941 
942  // on a donc maintenant les differents cas possibles
943  // on peut ensuite remonter aux deplacements
944  // on separe encore les cas 1,2,3
945  // la, deux choix se posent suivant le signe de d
946  if (distanceFictive > 0) {
947  if (cas == cas1) {
948  if (norm1ok) {
949 
950  // cos theta
951  Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
952 
953  Rprim1[2][2] = Rprim1[0][0];
954 
955  // sin theta
956  Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
957  ((sv[0] + sv[2]) * sv[1]);
958 
959  Rprim1[0][2] = -Rprim1[2][0];
960 
961  Rprim1[1][1] = 1.0;
962 
963  Tprim1[0] = Nprim1[0];
964  Tprim1[1] = 0.0;
965  Tprim1[2] = -Nprim1[2];
966 
967  Tprim1 *= (sv[0] - sv[2]);
968  }
969 
970  if (norm2ok) {
971 
972  // cos theta
973  Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
974 
975  Rprim2[2][2] = Rprim2[0][0];
976 
977  // sin theta
978  Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
979  ((sv[0] + sv[2]) * sv[1]);
980 
981  Rprim2[0][2] = -Rprim2[2][0];
982 
983  Rprim2[1][1] = 1.0;
984 
985  Tprim2[0] = Nprim2[0];
986  Tprim2[1] = 0.0;
987  Tprim2[2] = -Nprim2[2];
988 
989  Tprim2 *= (sv[0] - sv[2]);
990  }
991 
992  if (norm3ok) {
993 
994  // cos theta
995  Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
996 
997  Rprim3[2][2] = Rprim3[0][0];
998 
999  // sin theta
1000  Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1001  ((sv[0] + sv[2]) * sv[1]);
1002 
1003  Rprim3[0][2] = -Rprim3[2][0];
1004 
1005  Rprim3[1][1] = 1.0;
1006 
1007  Tprim3[0] = Nprim3[0];
1008  Tprim3[1] = 0.0;
1009  Tprim3[2] = -Nprim3[2];
1010 
1011  Tprim3 *= (sv[0] - sv[2]);
1012  }
1013 
1014  if (norm4ok) {
1015 
1016  // cos theta
1017  Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
1018 
1019  Rprim4[2][2] = Rprim4[0][0];
1020 
1021  // sin theta
1022  Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1023  ((sv[0] + sv[2]) * sv[1]);
1024 
1025  Rprim4[0][2] = -Rprim4[2][0];
1026 
1027  Rprim4[1][1] = 1.0;
1028 
1029  Tprim4[0] = Nprim4[0];
1030  Tprim4[1] = 0.0;
1031  Tprim4[2] = -Nprim4[2];
1032 
1033  Tprim4 *= (sv[0] - sv[2]);
1034  }
1035  }
1036 
1037  if (cas == cas2) {
1038  // 2 normales sont potentiellement candidates
1039 
1040  if (norm1ok) {
1041  Rprim1.eye();
1042 
1043  Tprim1 = Nprim1[0];
1044  Tprim1 *= (sv[2] - sv[0]);
1045  }
1046 
1047  if (norm2ok) {
1048  Rprim2.eye();
1049 
1050  Tprim2 = Nprim2[1];
1051  Tprim2 *= (sv[2] - sv[0]);
1052  }
1053  }
1054  if (cas == cas3) {
1055  if (norm1ok) {
1056  Rprim1.eye();
1057 
1058  Tprim1 = Nprim1[0];
1059  Tprim1 *= (sv[0] - sv[1]);
1060  }
1061 
1062  if (norm2ok) {
1063  Rprim2.eye();
1064 
1065  Tprim2 = Nprim2[1];
1066  Tprim2 *= (sv[0] - sv[1]);
1067  }
1068  }
1069  if (cas == cas4) {
1070  // on ne connait pas la normale dans ce cas la
1071  Rprim1.eye();
1072  Tprim1 = 0.0;
1073  }
1074  }
1075 
1076  if (distanceFictive < 0) {
1077 
1078  if (cas == cas1) {
1079  if (norm1ok) {
1080 
1081  // cos theta
1082  Rprim1[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1083 
1084  Rprim1[2][2] = -Rprim1[0][0];
1085 
1086  // sin theta
1087  Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1088  ((sv[0] - sv[2]) * sv[1]);
1089 
1090  Rprim1[0][2] = Rprim1[2][0];
1091 
1092  Rprim1[1][1] = -1.0;
1093 
1094  Tprim1[0] = Nprim1[0];
1095  Tprim1[1] = 0.0;
1096  Tprim1[2] = Nprim1[2];
1097 
1098  Tprim1 *= (sv[0] + sv[2]);
1099  }
1100 
1101  if (norm2ok) {
1102 
1103  // cos theta
1104  Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1105 
1106  Rprim2[2][2] = -Rprim2[0][0];
1107 
1108  // sin theta
1109  Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1110  ((sv[0] - sv[2]) * sv[1]);
1111 
1112  Rprim2[0][2] = Rprim2[2][0];
1113 
1114  Rprim2[1][1] = -1.0;
1115 
1116  Tprim2[0] = Nprim2[0];
1117  Tprim2[1] = 0.0;
1118  Tprim2[2] = Nprim2[2];
1119 
1120  Tprim2 *= (sv[0] + sv[2]);
1121  }
1122 
1123  if (norm3ok) {
1124 
1125  // cos theta
1126  Rprim3[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1127 
1128  Rprim3[2][2] = -Rprim3[0][0];
1129 
1130  // sin theta
1131  Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1132  ((sv[0] - sv[2]) * sv[1]);
1133 
1134  Rprim3[0][2] = Rprim3[2][0];
1135 
1136  Rprim3[1][1] = -1.0;
1137 
1138  Tprim3[0] = Nprim3[0];
1139  Tprim3[1] = 0.0;
1140  Tprim3[2] = Nprim3[2];
1141 
1142  Tprim3 *= (sv[0] + sv[2]);
1143  }
1144 
1145  if (norm4ok) {
1146  // cos theta
1147  Rprim4[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1148 
1149  Rprim4[2][2] = -Rprim4[0][0];
1150 
1151  // sin theta
1152  Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
1153  ((sv[0] - sv[2]) * sv[1]);
1154 
1155  Rprim4[0][2] = Rprim4[2][0];
1156 
1157  Rprim4[1][1] = -1.0;
1158 
1159  Tprim4[0] = Nprim4[0];
1160  Tprim4[1] = 0.0;
1161  Tprim4[2] = Nprim4[2];
1162 
1163  Tprim4 *= (sv[0] + sv[2]);
1164  }
1165  }
1166  if (cas == cas2) {
1167  // 2 normales sont potentiellement candidates
1168 
1169  if (norm1ok) {
1170  Rprim1.eye();
1171  Rprim1[0][0] = -1;
1172  Rprim1[1][1] = -1;
1173 
1174  Tprim1 = Nprim1[0];
1175  Tprim1 *= (sv[2] + sv[0]);
1176  }
1177 
1178  if (norm2ok) {
1179  Rprim2.eye();
1180  Rprim2[0][0] = -1;
1181  Rprim2[1][1] = -1;
1182 
1183  Tprim2 = Nprim2[1];
1184  Tprim2 *= (sv[2] + sv[0]);
1185  }
1186  }
1187  if (cas == cas3) {
1188  if (norm1ok) {
1189  Rprim1.eye();
1190  Rprim1[2][2] = -1;
1191  Rprim1[1][1] = -1;
1192 
1193  Tprim1 = Nprim1[0];
1194  Tprim1 *= (sv[2] + sv[0]);
1195  }
1196 
1197  if (norm2ok) {
1198  Rprim2.eye();
1199  Rprim2[2][2] = -1;
1200  Rprim2[1][1] = -1;
1201 
1202  Tprim2 = Nprim2[1];
1203  Tprim2 *= (sv[0] + sv[2]);
1204  }
1205  }
1206 
1207  // ON NE CONSIDERE PAS LE CAS NUMERO 4
1208  }
1209  // tous les Rprim et Tprim sont calcules
1210  // on peut maintenant recuperer la
1211  // rotation, et la translation
1212  // IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
1213  if ((distanceFictive > 0) || (cas != cas4)) {
1214  // on controle juste si les normales sont ok
1215 
1216  if (norm1ok) {
1217  R1 = s * U * Rprim1 * V.t();
1218  T1 = U * Tprim1;
1219  T1 /= (distanceFictive * s);
1220  N1 = V * Nprim1;
1221 
1222  // je rajoute le resultat
1223  vR.push_back(R1);
1224  vT.push_back(T1);
1225  vN.push_back(N1);
1226  }
1227  if (norm2ok) {
1228  R2 = s * U * Rprim2 * V.t();
1229  T2 = U * Tprim2;
1230  T2 /= (distanceFictive * s);
1231  N2 = V * Nprim2;
1232 
1233  // je rajoute le resultat
1234  vR.push_back(R2);
1235  vT.push_back(T2);
1236  vN.push_back(N2);
1237  }
1238  if (norm3ok) {
1239  R3 = s * U * Rprim3 * V.t();
1240  T3 = U * Tprim3;
1241  T3 /= (distanceFictive * s);
1242  N3 = V * Nprim3;
1243  // je rajoute le resultat
1244  vR.push_back(R3);
1245  vT.push_back(T3);
1246  vN.push_back(N3);
1247  }
1248  if (norm4ok) {
1249  R4 = s * U * Rprim4 * V.t();
1250  T4 = U * Tprim4;
1251  T4 /= (distanceFictive * s);
1252  N4 = V * Nprim4;
1253 
1254  // je rajoute le resultat
1255  vR.push_back(R4);
1256  vT.push_back(T4);
1257  vN.push_back(N4);
1258  }
1259  } else {
1260  std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas "
1261  "estimable!"
1262  << std::endl;
1263  }
1264 
1265  // on peut ensuite afficher les resultats...
1266  /* std::cout << "Analyse des resultats : "<< std::endl; */
1267  /* if (cas==cas1) */
1268  /* std::cout << "On est dans le cas 1" << std::endl; */
1269  /* if (cas==cas2) */
1270  /* std::cout << "On est dans le cas 2" << std::endl; */
1271  /* if (cas==cas3) */
1272  /* std::cout << "On est dans le cas 3" << std::endl; */
1273  /* if (cas==cas4) */
1274  /* std::cout << "On est dans le cas 4" << std::endl; */
1275 
1276  /* if (distanceFictive < 0) */
1277  /* std::cout << "d'<0" << std::endl; */
1278  /* else */
1279  /* std::cout << "d'>0" << std::endl; */
1280 
1281 #ifdef DEBUG_Homographie
1282  printf("fin : Homographie_EstimationDeplacementCamera\n");
1283 #endif
1284 }
1285 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
vpMatrix convert() const
static double sqr(double x)
Definition: vpMath.h:123
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2024
Implementation of a rotation matrix and operations on such kind of matrices.
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix t() const
Class that consider the case of a translation vector.
vpRowVector t() const