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