Visual Servoing Platform  version 3.6.1 under development (2024-10-14)
vpHomographyDLT.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 estimation.
32  */
33 
41 #include <visp3/core/vpMatrix.h>
42 #include <visp3/core/vpMatrixException.h>
43 #include <visp3/vision/vpHomography.h>
44 
45 #include <cmath> // std::fabs
46 #include <limits> // numeric_limits
47 
48 BEGIN_VISP_NAMESPACE
49 
50 #ifndef DOXYGEN_SHOULD_SKIP_THIS
51 
52 void vpHomography::hartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
53  std::vector<double> &xn, std::vector<double> &yn, double &xg, double &yg,
54  double &coef)
55 {
56  if (x.size() != y.size()) {
57  throw(vpException(vpException::dimensionError, "Hartley normalization require that x and y vector "
58  "have the same dimension"));
59  }
60 
61  unsigned int n = static_cast<unsigned int>(x.size());
62  if (xn.size() != n) {
63  xn.resize(n);
64  }
65  if (yn.size() != n) {
66  yn.resize(n);
67  }
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  }
97  else {
98  coef = sqrt(2.0) / distance;
99  }
100 
101  for (unsigned int i = 0; i < n; ++i) {
102  xn[i] *= coef;
103  yn[i] *= coef;
104  }
105 }
106 
107 void vpHomography::hartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn,
108  double &xg, double &yg, double &coef)
109 {
110  unsigned int i;
111  xg = 0;
112  yg = 0;
113 
114  for (i = 0; i < n; ++i) {
115  xg += x[i];
116  yg += y[i];
117  }
118  xg /= n;
119  yg /= n;
120 
121  // Changement d'origine : le centre de gravite doit correspondre
122  // a l'origine des coordonnees
123  double distance = 0;
124  for (i = 0; i < n; ++i) {
125  double xni = x[i] - xg;
126  double yni = y[i] - yg;
127  xn[i] = xni;
128  yn[i] = yni;
129  distance += sqrt(vpMath::sqr(xni) + vpMath::sqr(yni));
130  } // for translation sur tous les points
131 
132  // Changement d'echelle
133  distance /= n;
134  // calcul du coef de changement d'echelle
135  // if(distance ==0)
136  if (std::fabs(distance) <= std::numeric_limits<double>::epsilon()) {
137  coef = 1;
138  }
139  else {
140  coef = sqrt(2.0) / distance;
141  }
142 
143  for (i = 0; i < n; ++i) {
144  xn[i] *= coef;
145  yn[i] *= coef;
146  }
147 }
148 
149 //---------------------------------------------------------------------------------------
150 
151 void vpHomography::hartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, double xg1, double yg1, double coef1,
152  double xg2, double yg2, double coef2)
153 {
154 
155  // calcul des transformations a appliquer sur M_norm pour obtenir M
156  // en fonction des deux normalizations effectuees au debut sur
157  // les points: aHb = T2^ aHbn T1
158  vpMatrix T1(3, 3);
159  vpMatrix T2(3, 3);
160  vpMatrix T2T(3, 3);
161 
162  T1.eye();
163  T2.eye();
164  T2T.eye();
165 
166  T1[0][0] = (T1[1][1] = coef1);
167  T1[0][2] = -coef1 * xg1;
168  T1[1][2] = -coef1 * yg1;
169 
170  T2[0][0] = (T2[1][1] = coef2);
171  T2[0][2] = -coef2 * xg2;
172  T2[1][2] = -coef2 * yg2;
173 
174  T2T = T2.pseudoInverse(1e-16);
175 
176  vpMatrix aHbn_(3, 3);
177  const unsigned int val_3 = 3;
178  for (unsigned int i = 0; i < val_3; ++i) {
179  for (unsigned int j = 0; j < val_3; ++j) {
180  aHbn_[i][j] = aHbn[i][j];
181  }
182  }
183 
184  vpMatrix maHb = T2T * aHbn_ * T1;
185 
186  for (unsigned int i = 0; i < val_3; ++i) {
187  for (unsigned int j = 0; j < val_3; ++j) {
188  aHb[i][j] = maHb[i][j];
189  }
190  }
191 }
192 
193 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
194 
195 void vpHomography::DLT(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
196  const std::vector<double> &ya, vpHomography &aHb, bool normalization)
197 {
198  unsigned int n = static_cast<unsigned int>(xb.size());
199  if ((yb.size() != n) || (xa.size() != n) || (ya.size() != n)) {
200  throw(vpException(vpException::dimensionError, "Bad dimension for DLT homography estimation"));
201  }
202 
203  // 4 point are required
204  const unsigned int nbRequiredPoints = 4;
205  if (n < nbRequiredPoints) {
206  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
207  }
208 
209  std::vector<double> xan, yan, xbn, ybn;
210 
211  double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
212 
213  vpHomography aHbn;
214 
215  if (normalization) {
216  vpHomography::hartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
217  vpHomography::hartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
218  }
219  else {
220  xbn = xb;
221  ybn = yb;
222  xan = xa;
223  yan = ya;
224  }
225 
226  vpMatrix A(2 * n, 9);
227  vpColVector h(9);
228  vpColVector D(9);
229  vpMatrix V(9, 9);
230 
231  // We need here to compute the SVD on a (n*2)*9 matrix (where n is
232  // the number of points). if n == 4, the matrix has more columns
233  // than rows. This kind of matrix is not supported by GSL for
234  // SVD. The solution is to add an extra line with zeros
235  if (n == 4) {
236  A.resize((2 * n) + 1, 9);
237  }
238 
239  // build matrix A
240  for (unsigned int i = 0; i < n; ++i) {
241  A[2 * i][0] = 0;
242  A[2 * i][1] = 0;
243  A[2 * i][2] = 0;
244  A[2 * i][3] = -xbn[i];
245  A[2 * i][4] = -ybn[i];
246  A[2 * i][5] = -1;
247  A[2 * i][6] = xbn[i] * yan[i];
248  A[2 * i][7] = ybn[i] * yan[i];
249  A[2 * i][8] = yan[i];
250 
251  A[(2 * i) + 1][0] = xbn[i];
252  A[(2 * i) + 1][1] = ybn[i];
253  A[(2 * i) + 1][2] = 1;
254  A[(2 * i) + 1][3] = 0;
255  A[(2 * i) + 1][4] = 0;
256  A[(2 * i) + 1][5] = 0;
257  A[(2 * i) + 1][6] = -xbn[i] * xan[i];
258  A[(2 * i) + 1][7] = -ybn[i] * xan[i];
259  A[(2 * i) + 1][8] = -xan[i];
260  }
261 
262  // Add an extra line with zero.
263  if (n == 4) {
264  for (unsigned int i = 0; i < 9; ++i) {
265  A[2 * n][i] = 0;
266  }
267  }
268 
269  // solve Ah = 0
270  // SVD Decomposition A = UDV^T (destructive wrt A)
271  A.svd(D, V);
272 
273  // on en profite pour effectuer un controle sur le rang de la matrice :
274  // pas plus de 2 valeurs singulieres quasi=0
275  int rank = 0;
276  for (unsigned int i = 0; i < 9; ++i) {
277  if (D[i] > 1e-7) {
278  ++rank;
279  }
280  }
281  if (rank < 7) {
282  throw(vpMatrixException(vpMatrixException::rankDeficient, "Matrix rank %d is deficient (should be 8)", rank));
283  }
284  // h = is the column of V associated with the smallest singular value of A
285 
286  // since we are not sure that the svd implemented sort the
287  // singular value... we seek for the smallest
288  double smallestSv = 1e30;
289  unsigned int indexSmallestSv = 0;
290  for (unsigned int i = 0; i < 9; ++i) {
291  if (D[i] < smallestSv) {
292  smallestSv = D[i];
293  indexSmallestSv = i;
294  }
295  }
296 
297  h = V.getCol(indexSmallestSv);
298 
299  // build the homography
300  const unsigned int val_3 = 3;
301  for (unsigned int i = 0; i < val_3; ++i) {
302  for (unsigned int j = 0; j < val_3; ++j) {
303  aHbn[i][j] = h[(3 * i) + j];
304  }
305  }
306 
307  if (normalization) {
308  // H after denormalization
309  vpHomography::hartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
310  }
311  else {
312  aHb = aHbn;
313  }
314 }
315 
316 END_VISP_NAMESPACE
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
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)
static double sqr(double x)
Definition: vpMath.h:203
error that can be emitted by the vpMatrix class and its derivatives
@ rankDeficient
Rank deficient.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void svd(vpColVector &w, vpMatrix &V)
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:548