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