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