ViSP  2.10.0
vpHomographyDLT.cpp
1 /****************************************************************************
2  *
3  * $Id: vpHomographyDLT.cpp 5052 2014-12-11 14:18:49Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Homography estimation.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
50 #include <visp/vpHomography.h>
51 #include <visp/vpMatrix.h>
52 #include <visp/vpMatrixException.h>
53 
54 #include <cmath> // std::fabs
55 #include <limits> // numeric_limits
56 
57 #ifndef DOXYGEN_SHOULD_SKIP_THIS
58 
59 void
60 vpHomography::HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
61  std::vector<double> &xn, std::vector<double> &yn,
62  double &xg, double &yg, double &coef)
63 {
64  if (x.size() != y.size())
66  "Hartley normalization require that x and y vector have the same dimension"));
67 
68  unsigned int n = (unsigned int) x.size();
69  if (xn.size() != n)
70  xn.resize(n);
71  if (yn.size() != n)
72  yn.resize(n);
73 
74  xg = 0 ;
75  yg = 0 ;
76 
77  for (unsigned int i =0 ; i < n ; i++)
78  {
79  xg += x[i] ;
80  yg += y[i] ;
81  }
82  xg /= n ;
83  yg /= n ;
84 
85  // Changement d'origine : le centre de gravite doit correspondre
86  // a l'origine des coordonnees
87  double distance=0;
88  for(unsigned int i=0; i<n;i++)
89  {
90  double xni=x[i]-xg;
91  double yni=y[i]-yg;
92  xn[i] = xni ;
93  yn[i] = yni ;
94  distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
95  }//for translation sur tous les points
96 
97  //Changement d'echelle
98  distance/=n;
99  //calcul du coef de changement d'echelle
100  //if(distance ==0)
101  if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
102  coef=1;
103  else
104  coef=sqrt(2.0)/distance;
105 
106  for(unsigned int i=0; i<n;i++)
107  {
108  xn[i] *= coef;
109  yn[i] *= coef;
110  }
111 }
112 
113 void
114 vpHomography::HartleyNormalization(unsigned int n,
115  const double *x, const double *y,
116  double *xn, double *yn,
117  double &xg, double &yg,
118  double &coef)
119 {
120  unsigned int i;
121  xg = 0 ;
122  yg = 0 ;
123 
124  for (i =0 ; i < n ; i++)
125  {
126  xg += x[i] ;
127  yg += y[i] ;
128  }
129  xg /= n ;
130  yg /= n ;
131 
132  //Changement d'origine : le centre de gravite doit correspondre
133  // a l'origine des coordonnees
134  double distance=0;
135  for(i=0; i<n;i++)
136  {
137  double xni=x[i]-xg;
138  double yni=y[i]-yg;
139  xn[i] = xni ;
140  yn[i] = yni ;
141  distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
142  }//for translation sur tous les points
143 
144  //Changement d'echelle
145  distance/=n;
146  //calcul du coef de changement d'echelle
147  //if(distance ==0)
148  if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
149  coef=1;
150  else
151  coef=sqrt(2.0)/distance;
152 
153  for(i=0; i<n;i++)
154  {
155  xn[i] *= coef;
156  yn[i] *= coef;
157  }
158 }
159 
160 //---------------------------------------------------------------------------------------
161 
162 void
163 vpHomography::HartleyDenormalization(vpHomography &aHbn,
164  vpHomography &aHb,
165  double xg1, double yg1, double coef1,
166  double xg2, double yg2, double coef2 )
167 {
168 
169  //calcul des transformations a appliquer sur M_norm pour obtenir M
170  //en fonction des deux normalisations effectuees au debut sur
171  //les points: aHb = T2^ aHbn T1
172  vpMatrix T1(3,3);
173  vpMatrix T2(3,3);
174  vpMatrix T2T(3,3);
175 
176  T1.setIdentity();
177  T2.setIdentity();
178  T2T.setIdentity();
179 
180  T1[0][0]=T1[1][1]=coef1;
181  T1[0][2]=-coef1*xg1 ;
182  T1[1][2]=-coef1*yg1 ;
183 
184  T2[0][0]=T2[1][1]=coef2;
185  T2[0][2]=-coef2*xg2 ;
186  T2[1][2]=-coef2*yg2 ;
187 
188  T2T=T2.pseudoInverse(1e-16) ;
189 
190  vpMatrix aHbn_(3,3);
191  for(unsigned int i=0; i<3; i++)
192  for(unsigned int j=0; j<3; j++)
193  aHbn_[i][j] = aHbn[i][j];
194 
195  vpMatrix maHb=T2T*aHbn_*T1;
196 
197  for (unsigned int i=0 ; i < 3 ; i++)
198  for (unsigned int j=0 ; j < 3 ; j++)
199  aHb[i][j] = maHb[i][j] ;
200 }
201 
202 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
203 
204 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
205 
224 void
226  double *xb, double *yb,
227  double *xa, double *ya ,
228  vpHomography &aHb)
229 {
230  try{
231  //initialise les donnees initiales
232  // code_retour =InitialData(n, p1,p2);
233 
234  // normalize points
235  double *xbn;
236  double *ybn;
237  xbn = new double [n];
238  ybn = new double [n];
239 
240  double xg1, yg1, coef1 ;
241  vpHomography::HartleyNormalization(n,
242  xb,yb,
243  xbn,ybn,
244  xg1, yg1,coef1);
245 
246  double *xan;
247  double *yan;
248  xan = new double [n];
249  yan = new double [n];
250 
251  double xg2, yg2, coef2 ;
252  vpHomography::HartleyNormalization(n,
253  xa,ya,
254  xan,yan,
255  xg2, yg2,coef2);
256 
257  vpHomography aHbn ;
258  //compute the homography using the DLT from normalized data
259  vpHomography::DLT(n,xbn,ybn,xan,yan,aHbn);
260 
261  //H denormalisee
262  vpHomography::HartleyDenormalization(aHbn,aHb,xg1,yg1,coef1,xg2,yg2, coef2);
263 
264  delete [] xbn;
265  delete [] ybn;
266  delete [] xan;
267  delete [] yan;
268 
269  }
270  catch(...)
271  {
272  vpTRACE(" ") ;
273  throw ;
274  }
275 }
276 #endif //#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
277 
278 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
279 
338 void vpHomography::DLT(unsigned int n,
339  double *xb, double *yb,
340  double *xa, double *ya ,
341  vpHomography &aHb)
342 {
343 
344  // 4 point are required
345  if(n<4)
346  {
347  vpTRACE("there must be at least 4 points in the both images\n") ;
348  throw ;
349  }
350 
351  try{
352  vpMatrix A(2*n,9);
353  vpColVector h(9);
354  vpColVector D(9);
355  vpMatrix V(9,9);
356 
357  // We need here to compute the SVD on a (n*2)*9 matrix (where n is
358  // the number of points). if n == 4, the matrix has more columns
359  // than rows. This kind of matrix is not supported by GSL for
360  // SVD. The solution is to add an extra line with zeros
361  if (n == 4)
362  A.resize(2*n+1,9);
363 
364  // build matrix A
365  for(unsigned int i=0; i<n;i++)
366  {
367  A[2*i][0]=0;
368  A[2*i][1]=0;
369  A[2*i][2]=0;
370  A[2*i][3]=-xb[i] ;
371  A[2*i][4]=-yb[i] ;
372  A[2*i][5]=-1;
373  A[2*i][6]=xb[i]*ya[i] ;
374  A[2*i][7]=yb[i]*ya[i];
375  A[2*i][8]=ya[i];
376 
377 
378  A[2*i+1][0]=xb[i] ;
379  A[2*i+1][1]=yb[i] ;
380  A[2*i+1][2]=1;
381  A[2*i+1][3]=0;
382  A[2*i+1][4]=0;
383  A[2*i+1][5]=0;
384  A[2*i+1][6]=-xb[i]*xa[i];
385  A[2*i+1][7]=-yb[i]*xa[i];
386  A[2*i+1][8]=-xa[i] ;
387  }
388 
389  // Add an extra line with zero.
390  if (n == 4) {
391  for (unsigned int i=0; i < 9; i ++) {
392  A[2*n][i] = 0;
393  }
394  }
395 
396  // solve Ah = 0
397  // SVD Decomposition A = UDV^T (destructive wrt A)
398  A.svd(D,V);
399 
400  // on en profite pour effectuer un controle sur le rang de la matrice :
401  // pas plus de 2 valeurs singulieres quasi=0
402  int rank=0;
403  for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
404  if(rank <7)
405  {
406  vpTRACE(" Rank is : %d, should be 8", rank);
408  "\n\t\t Matrix rank is deficient")) ;
409  }
410  //h = is the column of V associated with the smallest singular value of A
411 
412  // since we are not sure that the svd implemented sort the
413  // singular value... we seek for the smallest
414  double smallestSv = 1e30 ;
415  unsigned int indexSmallestSv = 0 ;
416  for (unsigned int i=0 ; i < 9 ; i++)
417  if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }
418 
419 
420  h=V.getCol(indexSmallestSv);
421 
422  // build the homography
423  for(unsigned int i =0;i<3;i++)
424  {
425  for(unsigned int j=0;j<3;j++)
426  aHb[i][j]=h[3*i+j];
427  }
428 
429  }
430  catch(vpMatrixException me)
431  {
432  vpTRACE("Matrix Exception ") ;
433  throw(me) ;
434  }
435  catch(vpException me)
436  {
437  vpERROR_TRACE("caught another error") ;
438  std::cout <<std::endl << me << std::endl ;
439  throw(me) ;
440  }
441 
442 }
443 #endif // #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
444 
503 void vpHomography::DLT(const std::vector<double> &xb, const std::vector<double> &yb,
504  const std::vector<double> &xa, const std::vector<double> &ya ,
505  vpHomography &aHb,
506  bool normalization)
507 {
508  unsigned int n = (unsigned int) xb.size();
509  if (yb.size() != n || xa.size() != n || ya.size() != n)
511  "Bad dimension for DLT homography estimation"));
512 
513  // 4 point are required
514  if(n<4)
515  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
516 
517  try{
518  std::vector<double> xan, yan, xbn, ybn;
519 
520  double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
521 
522  vpHomography aHbn;
523 
524  if (normalization) {
525  vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
526  vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
527  }
528  else {
529  xbn = xb;
530  ybn = yb;
531  xan = xa;
532  yan = ya;
533  }
534 
535  vpMatrix A(2*n,9);
536  vpColVector h(9);
537  vpColVector D(9);
538  vpMatrix V(9,9);
539 
540  // We need here to compute the SVD on a (n*2)*9 matrix (where n is
541  // the number of points). if n == 4, the matrix has more columns
542  // than rows. This kind of matrix is not supported by GSL for
543  // SVD. The solution is to add an extra line with zeros
544  if (n == 4)
545  A.resize(2*n+1,9);
546 
547  // build matrix A
548  for(unsigned int i=0; i<n;i++)
549  {
550  A[2*i][0]=0;
551  A[2*i][1]=0;
552  A[2*i][2]=0;
553  A[2*i][3]=-xbn[i] ;
554  A[2*i][4]=-ybn[i] ;
555  A[2*i][5]=-1;
556  A[2*i][6]=xbn[i]*yan[i] ;
557  A[2*i][7]=ybn[i]*yan[i];
558  A[2*i][8]=yan[i];
559 
560 
561  A[2*i+1][0]=xbn[i] ;
562  A[2*i+1][1]=ybn[i] ;
563  A[2*i+1][2]=1;
564  A[2*i+1][3]=0;
565  A[2*i+1][4]=0;
566  A[2*i+1][5]=0;
567  A[2*i+1][6]=-xbn[i]*xan[i];
568  A[2*i+1][7]=-ybn[i]*xan[i];
569  A[2*i+1][8]=-xan[i] ;
570  }
571 
572  // Add an extra line with zero.
573  if (n == 4) {
574  for (unsigned int i=0; i < 9; i ++) {
575  A[2*n][i] = 0;
576  }
577  }
578 
579  // solve Ah = 0
580  // SVD Decomposition A = UDV^T (destructive wrt A)
581  A.svd(D,V);
582 
583  // on en profite pour effectuer un controle sur le rang de la matrice :
584  // pas plus de 2 valeurs singulieres quasi=0
585  int rank=0;
586  for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
587  if(rank <7)
588  {
589  vpTRACE(" Rank is : %d, should be 8", rank);
591  "\n\t\t Matrix rank is deficient")) ;
592  }
593  //h = is the column of V associated with the smallest singular value of A
594 
595  // since we are not sure that the svd implemented sort the
596  // singular value... we seek for the smallest
597  double smallestSv = 1e30 ;
598  unsigned int indexSmallestSv = 0 ;
599  for (unsigned int i=0 ; i < 9 ; i++)
600  if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }
601 
602  h=V.getCol(indexSmallestSv);
603 
604  // build the homography
605  for(unsigned int i =0;i<3;i++)
606  {
607  for(unsigned int j=0;j<3;j++)
608  aHbn[i][j]=h[3*i+j];
609  }
610 
611  if (normalization) {
612  // H after denormalization
613  vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
614  }
615  else {
616  aHb = aHbn;
617  }
618 
619  }
620  catch(vpMatrixException me)
621  {
622  vpTRACE("Matrix Exception ") ;
623  throw(me) ;
624  }
625  catch(vpException me)
626  {
627  vpERROR_TRACE("caught another error") ;
628  std::cout <<std::endl << me << std::endl ;
629  throw(me) ;
630  }
631 }
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpTRACE
Definition: vpDebug.h:418
error that can be emited by ViSP classes.
Definition: vpException.h:76
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:178
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1822
static double sqr(double x)
Definition: vpMath.h:106
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:2463
static void DLT(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, bool normalization=true)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
error that can be emited by the vpMatrix class and its derivates
static vp_deprecated void HartleyDLT(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpHomography &aHb)
Computes the homography matrix using the DLT (Direct Linear Transform) algorithm on normalized data...