1 /***************************************************************************
2 qgsleastsquares.cpp
3 --------------------------------------
4 Date : Sun Sep 16 12:03:37 AKDT 2007
5 Copyright : (C) 2007 by Gary E. Sherman
6 Email : sherman at mrcc dot com
7 ***************************************************************************
8 * *
9 * This program is free software; you can redistribute it and/or modify *
10 * it under the terms of the GNU General Public License as published by *
11 * the Free Software Foundation; either version 2 of the License, or *
12 * (at your option) any later version. *
13 * *
14 ***************************************************************************/
15 #include <cmath>
16 #include <stdexcept>
17
18 #include <gsl/gsl_linalg.h>
19 #include <gsl/gsl_blas.h>
20
21 #include <QObject>
22
23 #include "qgsleastsquares.h"
24
25
linear(const QVector<QgsPointXY> & mapCoords,const QVector<QgsPointXY> & pixelCoords,QgsPointXY & origin,double & pixelXSize,double & pixelYSize)26 void QgsLeastSquares::linear( const QVector<QgsPointXY> &mapCoords,
27 const QVector<QgsPointXY> &pixelCoords,
28 QgsPointXY &origin, double &pixelXSize, double &pixelYSize )
29 {
30 int n = mapCoords.size();
31 if ( n < 2 )
32 {
33 throw std::domain_error( QObject::tr( "Fit to a linear transform requires at least 2 points." ).toLocal8Bit().constData() );
34 }
35
36 double sumPx( 0 ), sumPy( 0 ), sumPx2( 0 ), sumPy2( 0 ), sumPxMx( 0 ), sumPyMy( 0 ), sumMx( 0 ), sumMy( 0 );
37 for ( int i = 0; i < n; ++i )
38 {
39 sumPx += pixelCoords.at( i ).x();
40 sumPy += pixelCoords.at( i ).y();
41 sumPx2 += std::pow( pixelCoords.at( i ).x(), 2 );
42 sumPy2 += std::pow( pixelCoords.at( i ).y(), 2 );
43 sumPxMx += pixelCoords.at( i ).x() * mapCoords.at( i ).x();
44 sumPyMy += pixelCoords.at( i ).y() * mapCoords.at( i ).y();
45 sumMx += mapCoords.at( i ).x();
46 sumMy += mapCoords.at( i ).y();
47 }
48
49 double deltaX = n * sumPx2 - std::pow( sumPx, 2 );
50 double deltaY = n * sumPy2 - std::pow( sumPy, 2 );
51
52 double aX = ( sumPx2 * sumMx - sumPx * sumPxMx ) / deltaX;
53 double aY = ( sumPy2 * sumMy - sumPy * sumPyMy ) / deltaY;
54 double bX = ( n * sumPxMx - sumPx * sumMx ) / deltaX;
55 double bY = ( n * sumPyMy - sumPy * sumMy ) / deltaY;
56
57 origin.setX( aX );
58 origin.setY( aY );
59
60 pixelXSize = std::fabs( bX );
61 pixelYSize = std::fabs( bY );
62 }
63
64
helmert(const QVector<QgsPointXY> & mapCoords,const QVector<QgsPointXY> & pixelCoords,QgsPointXY & origin,double & pixelSize,double & rotation)65 void QgsLeastSquares::helmert( const QVector<QgsPointXY> &mapCoords,
66 const QVector<QgsPointXY> &pixelCoords,
67 QgsPointXY &origin, double &pixelSize,
68 double &rotation )
69 {
70 int n = mapCoords.size();
71 if ( n < 2 )
72 {
73 throw std::domain_error( QObject::tr( "Fit to a Helmert transform requires at least 2 points." ).toLocal8Bit().constData() );
74 }
75
76 double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0, G = 0, H = 0, I = 0, J = 0;
77 for ( int i = 0; i < n; ++i )
78 {
79 A += pixelCoords.at( i ).x();
80 B += pixelCoords.at( i ).y();
81 C += mapCoords.at( i ).x();
82 D += mapCoords.at( i ).y();
83 E += mapCoords.at( i ).x() * pixelCoords.at( i ).x();
84 F += mapCoords.at( i ).y() * pixelCoords.at( i ).y();
85 G += std::pow( pixelCoords.at( i ).x(), 2 );
86 H += std::pow( pixelCoords.at( i ).y(), 2 );
87 I += mapCoords.at( i ).x() * pixelCoords.at( i ).y();
88 J += pixelCoords.at( i ).x() * mapCoords.at( i ).y();
89 }
90
91 /* The least squares fit for the parameters { a, b, x0, y0 } is the solution
92 to the matrix equation Mx = b, where M and b is given below. I *think*
93 that this is correct but I derived it myself late at night. Look at
94 helmert.jpg if you suspect bugs. */
95
96 double MData[] = { A, -B, ( double ) n, 0.,
97 B, A, 0., ( double ) n,
98 G + H, 0., A, B,
99 0., G + H, -B, A
100 };
101
102 double bData[] = { C, D, E + F, J - I };
103
104 // we want to solve the equation M*x = b, where x = [a b x0 y0]
105 gsl_matrix_view M = gsl_matrix_view_array( MData, 4, 4 );
106 gsl_vector_view b = gsl_vector_view_array( bData, 4 );
107 gsl_vector *x = gsl_vector_alloc( 4 );
108 gsl_permutation *p = gsl_permutation_alloc( 4 );
109 int s;
110 gsl_linalg_LU_decomp( &M.matrix, p, &s );
111 gsl_linalg_LU_solve( &M.matrix, p, &b.vector, x );
112 gsl_permutation_free( p );
113
114 origin.setX( gsl_vector_get( x, 2 ) );
115 origin.setY( gsl_vector_get( x, 3 ) );
116 pixelSize = std::sqrt( std::pow( gsl_vector_get( x, 0 ), 2 ) +
117 std::pow( gsl_vector_get( x, 1 ), 2 ) );
118 rotation = std::atan2( gsl_vector_get( x, 1 ), gsl_vector_get( x, 0 ) );
119 }
120
121
affine(QVector<QgsPointXY> mapCoords,QVector<QgsPointXY> pixelCoords)122 void QgsLeastSquares::affine( QVector<QgsPointXY> mapCoords,
123 QVector<QgsPointXY> pixelCoords )
124 {
125 int n = mapCoords.size();
126 if ( n < 4 )
127 {
128 throw std::domain_error( QObject::tr( "Fit to an affine transform requires at least 4 points." ).toLocal8Bit().constData() );
129 }
130
131 double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0,
132 G = 0, H = 0, I = 0, J = 0, K = 0;
133 for ( int i = 0; i < n; ++i )
134 {
135 A += pixelCoords[i].x();
136 B += pixelCoords[i].y();
137 C += mapCoords[i].x();
138 D += mapCoords[i].y();
139 E += std::pow( pixelCoords[i].x(), 2 );
140 F += std::pow( pixelCoords[i].y(), 2 );
141 G += pixelCoords[i].x() * pixelCoords[i].y();
142 H += pixelCoords[i].x() * mapCoords[i].x();
143 I += pixelCoords[i].y() * mapCoords[i].y();
144 J += pixelCoords[i].x() * mapCoords[i].y();
145 K += mapCoords[i].x() * pixelCoords[i].y();
146 }
147
148 /* The least squares fit for the parameters { a, b, c, d, x0, y0 } is the
149 solution to the matrix equation Mx = b, where M and b is given below.
150 I *think* that this is correct but I derived it myself late at night.
151 Look at affine.jpg if you suspect bugs. */
152
153 double MData[] = { A, B, 0, 0, ( double ) n, 0,
154 0, 0, A, B, 0, ( double ) n,
155 E, G, 0, 0, A, 0,
156 G, F, 0, 0, B, 0,
157 0, 0, E, G, 0, A,
158 0, 0, G, F, 0, B
159 };
160
161 double bData[] = { C, D, H, K, J, I };
162
163 // we want to solve the equation M*x = b, where x = [a b c d x0 y0]
164 gsl_matrix_view M = gsl_matrix_view_array( MData, 6, 6 );
165 gsl_vector_view b = gsl_vector_view_array( bData, 6 );
166 gsl_vector *x = gsl_vector_alloc( 6 );
167 gsl_permutation *p = gsl_permutation_alloc( 6 );
168 int s;
169 gsl_linalg_LU_decomp( &M.matrix, p, &s );
170 gsl_linalg_LU_solve( &M.matrix, p, &b.vector, x );
171 gsl_permutation_free( p );
172
173 }
174
175
176 /**
177 * Scales the given coordinates so that the center of gravity is at the origin and the mean distance to the origin is sqrt(2).
178 *
179 * Also returns 3x3 homogeneous matrices which can be used to normalize and de-normalize coordinates.
180 */
normalizeCoordinates(const QVector<QgsPointXY> & coords,QVector<QgsPointXY> & normalizedCoords,double normalizeMatrix[9],double denormalizeMatrix[9])181 void normalizeCoordinates( const QVector<QgsPointXY> &coords, QVector<QgsPointXY> &normalizedCoords,
182 double normalizeMatrix[9], double denormalizeMatrix[9] )
183 {
184 // Calculate center of gravity
185 double cogX = 0.0, cogY = 0.0;
186 for ( int i = 0; i < coords.size(); i++ )
187 {
188 cogX += coords[i].x();
189 cogY += coords[i].y();
190 }
191 cogX *= 1.0 / coords.size();
192 cogY *= 1.0 / coords.size();
193
194 // Calculate mean distance to origin
195 double meanDist = 0.0;
196 for ( int i = 0; i < coords.size(); i++ )
197 {
198 double X = ( coords[i].x() - cogX );
199 double Y = ( coords[i].y() - cogY );
200 meanDist += std::sqrt( X * X + Y * Y );
201 }
202 meanDist *= 1.0 / coords.size();
203
204 double OOD = meanDist * M_SQRT1_2;
205 double D = 1.0 / OOD;
206 normalizedCoords.resize( coords.size() );
207 for ( int i = 0; i < coords.size(); i++ )
208 {
209 normalizedCoords[i] = QgsPointXY( ( coords[i].x() - cogX ) * D, ( coords[i].y() - cogY ) * D );
210 }
211
212 normalizeMatrix[0] = D;
213 normalizeMatrix[1] = 0.0;
214 normalizeMatrix[2] = -cogX * D;
215 normalizeMatrix[3] = 0.0;
216 normalizeMatrix[4] = D;
217 normalizeMatrix[5] = -cogY * D;
218 normalizeMatrix[6] = 0.0;
219 normalizeMatrix[7] = 0.0;
220 normalizeMatrix[8] = 1.0;
221
222 denormalizeMatrix[0] = OOD;
223 denormalizeMatrix[1] = 0.0;
224 denormalizeMatrix[2] = cogX;
225 denormalizeMatrix[3] = 0.0;
226 denormalizeMatrix[4] = OOD;
227 denormalizeMatrix[5] = cogY;
228 denormalizeMatrix[6] = 0.0;
229 denormalizeMatrix[7] = 0.0;
230 denormalizeMatrix[8] = 1.0;
231 }
232
233 // Fits a homography to the given corresponding points, and
234 // return it in H (row-major format).
projective(QVector<QgsPointXY> mapCoords,QVector<QgsPointXY> pixelCoords,double H[9])235 void QgsLeastSquares::projective( QVector<QgsPointXY> mapCoords,
236 QVector<QgsPointXY> pixelCoords,
237 double H[9] )
238 {
239 Q_ASSERT( mapCoords.size() == pixelCoords.size() );
240
241 if ( mapCoords.size() < 4 )
242 {
243 throw std::domain_error( QObject::tr( "Fitting a projective transform requires at least 4 corresponding points." ).toLocal8Bit().constData() );
244 }
245
246 QVector<QgsPointXY> mapCoordsNormalized;
247 QVector<QgsPointXY> pixelCoordsNormalized;
248
249 double normMap[9], denormMap[9];
250 double normPixel[9], denormPixel[9];
251 normalizeCoordinates( mapCoords, mapCoordsNormalized, normMap, denormMap );
252 normalizeCoordinates( pixelCoords, pixelCoordsNormalized, normPixel, denormPixel );
253 mapCoords = mapCoordsNormalized;
254 pixelCoords = pixelCoordsNormalized;
255
256 // GSL does not support a full SVD, so we artificially add a linear dependent row
257 // to the matrix in case the system is underconstrained.
258 uint m = std::max( 9u, ( uint )mapCoords.size() * 2u );
259 uint n = 9;
260 gsl_matrix *S = gsl_matrix_alloc( m, n );
261
262 for ( int i = 0; i < mapCoords.size(); i++ )
263 {
264 gsl_matrix_set( S, i * 2, 0, pixelCoords[i].x() );
265 gsl_matrix_set( S, i * 2, 1, pixelCoords[i].y() );
266 gsl_matrix_set( S, i * 2, 2, 1.0 );
267
268 gsl_matrix_set( S, i * 2, 3, 0.0 );
269 gsl_matrix_set( S, i * 2, 4, 0.0 );
270 gsl_matrix_set( S, i * 2, 5, 0.0 );
271
272 gsl_matrix_set( S, i * 2, 6, -mapCoords[i].x()*pixelCoords[i].x() );
273 gsl_matrix_set( S, i * 2, 7, -mapCoords[i].x()*pixelCoords[i].y() );
274 gsl_matrix_set( S, i * 2, 8, -mapCoords[i].x() * 1.0 );
275
276 gsl_matrix_set( S, i * 2 + 1, 0, 0.0 );
277 gsl_matrix_set( S, i * 2 + 1, 1, 0.0 );
278 gsl_matrix_set( S, i * 2 + 1, 2, 0.0 );
279
280 gsl_matrix_set( S, i * 2 + 1, 3, pixelCoords[i].x() );
281 gsl_matrix_set( S, i * 2 + 1, 4, pixelCoords[i].y() );
282 gsl_matrix_set( S, i * 2 + 1, 5, 1.0 );
283
284 gsl_matrix_set( S, i * 2 + 1, 6, -mapCoords[i].y()*pixelCoords[i].x() );
285 gsl_matrix_set( S, i * 2 + 1, 7, -mapCoords[i].y()*pixelCoords[i].y() );
286 gsl_matrix_set( S, i * 2 + 1, 8, -mapCoords[i].y() * 1.0 );
287 }
288
289 if ( mapCoords.size() == 4 )
290 {
291 // The GSL SVD routine only supports matrices with rows >= columns (m >= n)
292 // Unfortunately, we can't use the SVD of the transpose (i.e. S^T = (U D V^T)^T = V D U^T)
293 // to work around this, because the solution lies in the right nullspace of S, and
294 // gsl only supports a thin SVD of S^T, which does not return these vectors.
295
296 // HACK: duplicate last row to get a 9x9 equation system
297 for ( int j = 0; j < 9; j++ )
298 {
299 gsl_matrix_set( S, 8, j, gsl_matrix_get( S, 7, j ) );
300 }
301 }
302
303 // Solve Sh = 0 in the total least squares sense, i.e.
304 // with Sh = min and |h|=1. The solution "h" is given by the
305 // right singular eigenvector of S corresponding, to the smallest
306 // singular value (via SVD).
307 gsl_matrix *V = gsl_matrix_alloc( n, n );
308 gsl_vector *singular_values = gsl_vector_alloc( n );
309 gsl_vector *work = gsl_vector_alloc( n );
310
311 // V = n x n
312 // U = m x n (thin SVD) U D V^T
313 gsl_linalg_SV_decomp( S, V, singular_values, work );
314
315 // Columns of V store the right singular vectors of S
316 for ( unsigned int i = 0; i < n; i++ )
317 {
318 H[i] = gsl_matrix_get( V, i, n - 1 );
319 }
320
321 gsl_matrix *prodMatrix = gsl_matrix_alloc( 3, 3 );
322
323 gsl_matrix_view Hmatrix = gsl_matrix_view_array( H, 3, 3 );
324 gsl_matrix_view normPixelMatrix = gsl_matrix_view_array( normPixel, 3, 3 );
325 gsl_matrix_view denormMapMatrix = gsl_matrix_view_array( denormMap, 3, 3 );
326
327 // Change coordinate frame of image and pre-image from normalized to map and pixel coordinates.
328 // H' = denormalizeMapCoords*H*normalizePixelCoords
329 gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1.0, &Hmatrix.matrix, &normPixelMatrix.matrix, 0.0, prodMatrix );
330 gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1.0, &denormMapMatrix.matrix, prodMatrix, 0.0, &Hmatrix.matrix );
331
332 gsl_matrix_free( prodMatrix );
333 gsl_matrix_free( S );
334 gsl_matrix_free( V );
335 gsl_vector_free( singular_values );
336 gsl_vector_free( work );
337 }
338