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