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