Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpHomographyDLT.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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 estimation.
32  *
33  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
46 #include <visp3/vision/vpHomography.h>
47 #include <visp3/core/vpMatrix.h>
48 #include <visp3/core/vpMatrixException.h>
49 
50 #include <cmath> // std::fabs
51 #include <limits> // numeric_limits
52 
53 #ifndef DOXYGEN_SHOULD_SKIP_THIS
54 
55 void
56 vpHomography::HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
57  std::vector<double> &xn, std::vector<double> &yn,
58  double &xg, double &yg, double &coef)
59 {
60  if (x.size() != y.size())
62  "Hartley normalization require that x and y vector have the same dimension"));
63 
64  unsigned int n = (unsigned int) x.size();
65  if (xn.size() != n)
66  xn.resize(n);
67  if (yn.size() != n)
68  yn.resize(n);
69 
70  xg = 0 ;
71  yg = 0 ;
72 
73  for (unsigned int i =0 ; i < n ; i++)
74  {
75  xg += x[i] ;
76  yg += y[i] ;
77  }
78  xg /= n ;
79  yg /= n ;
80 
81  // Changement d'origine : le centre de gravite doit correspondre
82  // a l'origine des coordonnees
83  double distance=0;
84  for(unsigned int i=0; i<n;i++)
85  {
86  double xni=x[i]-xg;
87  double yni=y[i]-yg;
88  xn[i] = xni ;
89  yn[i] = yni ;
90  distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
91  }//for translation sur tous les points
92 
93  //Changement d'echelle
94  distance/=n;
95  //calcul du coef de changement d'echelle
96  //if(distance ==0)
97  if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
98  coef=1;
99  else
100  coef=sqrt(2.0)/distance;
101 
102  for(unsigned int i=0; i<n;i++)
103  {
104  xn[i] *= coef;
105  yn[i] *= coef;
106  }
107 }
108 
109 void
110 vpHomography::HartleyNormalization(unsigned int n,
111  const double *x, const double *y,
112  double *xn, double *yn,
113  double &xg, double &yg,
114  double &coef)
115 {
116  unsigned int i;
117  xg = 0 ;
118  yg = 0 ;
119 
120  for (i =0 ; i < n ; i++)
121  {
122  xg += x[i] ;
123  yg += y[i] ;
124  }
125  xg /= n ;
126  yg /= n ;
127 
128  //Changement d'origine : le centre de gravite doit correspondre
129  // a l'origine des coordonnees
130  double distance=0;
131  for(i=0; i<n;i++)
132  {
133  double xni=x[i]-xg;
134  double yni=y[i]-yg;
135  xn[i] = xni ;
136  yn[i] = yni ;
137  distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
138  }//for translation sur tous les points
139 
140  //Changement d'echelle
141  distance/=n;
142  //calcul du coef de changement d'echelle
143  //if(distance ==0)
144  if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
145  coef=1;
146  else
147  coef=sqrt(2.0)/distance;
148 
149  for(i=0; i<n;i++)
150  {
151  xn[i] *= coef;
152  yn[i] *= coef;
153  }
154 }
155 
156 //---------------------------------------------------------------------------------------
157 
158 void
159 vpHomography::HartleyDenormalization(vpHomography &aHbn,
160  vpHomography &aHb,
161  double xg1, double yg1, double coef1,
162  double xg2, double yg2, double coef2 )
163 {
164 
165  //calcul des transformations a appliquer sur M_norm pour obtenir M
166  //en fonction des deux normalisations effectuees au debut sur
167  //les points: aHb = T2^ aHbn T1
168  vpMatrix T1(3,3);
169  vpMatrix T2(3,3);
170  vpMatrix T2T(3,3);
171 
172  T1.eye();
173  T2.eye();
174  T2T.eye();
175 
176  T1[0][0]=T1[1][1]=coef1;
177  T1[0][2]=-coef1*xg1 ;
178  T1[1][2]=-coef1*yg1 ;
179 
180  T2[0][0]=T2[1][1]=coef2;
181  T2[0][2]=-coef2*xg2 ;
182  T2[1][2]=-coef2*yg2 ;
183 
184  T2T=T2.pseudoInverse(1e-16) ;
185 
186  vpMatrix aHbn_(3,3);
187  for(unsigned int i=0; i<3; i++)
188  for(unsigned int j=0; j<3; j++)
189  aHbn_[i][j] = aHbn[i][j];
190 
191  vpMatrix maHb=T2T*aHbn_*T1;
192 
193  for (unsigned int i=0 ; i < 3 ; i++)
194  for (unsigned int j=0 ; j < 3 ; j++)
195  aHb[i][j] = maHb[i][j] ;
196 }
197 
198 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
199 
258 void vpHomography::DLT(const std::vector<double> &xb, const std::vector<double> &yb,
259  const std::vector<double> &xa, const std::vector<double> &ya ,
260  vpHomography &aHb,
261  bool normalization)
262 {
263  unsigned int n = (unsigned int) xb.size();
264  if (yb.size() != n || xa.size() != n || ya.size() != n)
266  "Bad dimension for DLT homography estimation"));
267 
268  // 4 point are required
269  if(n<4)
270  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
271 
272  try{
273  std::vector<double> xan, yan, xbn, ybn;
274 
275  double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
276 
277  vpHomography aHbn;
278 
279  if (normalization) {
280  vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
281  vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
282  }
283  else {
284  xbn = xb;
285  ybn = yb;
286  xan = xa;
287  yan = ya;
288  }
289 
290  vpMatrix A(2*n,9);
291  vpColVector h(9);
292  vpColVector D(9);
293  vpMatrix V(9,9);
294 
295  // We need here to compute the SVD on a (n*2)*9 matrix (where n is
296  // the number of points). if n == 4, the matrix has more columns
297  // than rows. This kind of matrix is not supported by GSL for
298  // SVD. The solution is to add an extra line with zeros
299  if (n == 4)
300  A.resize(2*n+1,9);
301 
302  // build matrix A
303  for(unsigned int i=0; i<n;i++)
304  {
305  A[2*i][0]=0;
306  A[2*i][1]=0;
307  A[2*i][2]=0;
308  A[2*i][3]=-xbn[i] ;
309  A[2*i][4]=-ybn[i] ;
310  A[2*i][5]=-1;
311  A[2*i][6]=xbn[i]*yan[i] ;
312  A[2*i][7]=ybn[i]*yan[i];
313  A[2*i][8]=yan[i];
314 
315 
316  A[2*i+1][0]=xbn[i] ;
317  A[2*i+1][1]=ybn[i] ;
318  A[2*i+1][2]=1;
319  A[2*i+1][3]=0;
320  A[2*i+1][4]=0;
321  A[2*i+1][5]=0;
322  A[2*i+1][6]=-xbn[i]*xan[i];
323  A[2*i+1][7]=-ybn[i]*xan[i];
324  A[2*i+1][8]=-xan[i] ;
325  }
326 
327  // Add an extra line with zero.
328  if (n == 4) {
329  for (unsigned int i=0; i < 9; i ++) {
330  A[2*n][i] = 0;
331  }
332  }
333 
334  // solve Ah = 0
335  // SVD Decomposition A = UDV^T (destructive wrt A)
336  A.svd(D,V);
337 
338  // on en profite pour effectuer un controle sur le rang de la matrice :
339  // pas plus de 2 valeurs singulieres quasi=0
340  int rank=0;
341  for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
342  if(rank <7)
343  {
344  vpTRACE(" Rank is : %d, should be 8", rank);
346  "\n\t\t Matrix rank is deficient")) ;
347  }
348  //h = is the column of V associated with the smallest singular value of A
349 
350  // since we are not sure that the svd implemented sort the
351  // singular value... we seek for the smallest
352  double smallestSv = 1e30 ;
353  unsigned int indexSmallestSv = 0 ;
354  for (unsigned int i=0 ; i < 9 ; i++)
355  if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }
356 
357  h=V.getCol(indexSmallestSv);
358 
359  // build the homography
360  for(unsigned int i =0;i<3;i++)
361  {
362  for(unsigned int j=0;j<3;j++)
363  aHbn[i][j]=h[3*i+j];
364  }
365 
366  if (normalization) {
367  // H after denormalization
368  vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
369  }
370  else {
371  aHb = aHbn;
372  }
373 
374  }
375  catch(vpMatrixException &me)
376  {
377  vpTRACE("Matrix Exception ") ;
378  throw(me) ;
379  }
380  catch(vpException &me)
381  {
382  vpERROR_TRACE("caught another error") ;
383  std::cout <<std::endl << me << std::endl ;
384  throw(me) ;
385  }
386 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
#define vpERROR_TRACE
Definition: vpDebug.h:391
error that can be emited by ViSP classes.
Definition: vpException.h:73
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1631
#define vpTRACE
Definition: vpDebug.h:414
static double sqr(double x)
Definition: vpMath.h:110
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:2235
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)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
error that can be emited by the vpMatrix class and its derivates