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