Visual Servoing Platform version 3.5.0
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
55void 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
105void 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
147void 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
250void 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 resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
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:116
error that can be emited by the vpMatrix class and its derivates
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393