QGIS API Documentation  3.20.0-Odense (decaadbb31)
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 
30 void QgsLeastSquares::linear( const QVector<QgsPointXY> &sourceCoordinates,
31  const QVector<QgsPointXY> &destinationCoordinates,
32  QgsPointXY &origin, double &pixelXSize, double &pixelYSize )
33 {
34  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  double deltaX = n * sumPx2 - std::pow( sumPx, 2 );
54  double deltaY = n * sumPy2 - std::pow( sumPy, 2 );
55 
56  double aX = ( sumPx2 * sumMx - sumPx * sumPxMx ) / deltaX;
57  double aY = ( sumPy2 * sumMy - sumPy * sumPyMy ) / deltaY;
58  double bX = ( n * sumPxMx - sumPx * sumMx ) / deltaX;
59  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 
69 void 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( QStringLiteral( "Calculating a helmert transformation requires a QGIS build based GSL" ) );
81 #else
82  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  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 #endif
141 }
142 
143 #if 0
144 void QgsLeastSquares::affine( QVector<QgsPointXY> mapCoords,
145  QVector<QgsPointXY> pixelCoords )
146 {
147  int n = mapCoords.size();
148  if ( n < 4 )
149  {
150  throw std::domain_error( QObject::tr( "Fit to an affine transform requires at least 4 points." ).toLocal8Bit().constData() );
151  }
152 
153  double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0,
154  G = 0, H = 0, I = 0, J = 0, K = 0;
155  for ( int i = 0; i < n; ++i )
156  {
157  A += pixelCoords[i].x();
158  B += pixelCoords[i].y();
159  C += mapCoords[i].x();
160  D += mapCoords[i].y();
161  E += std::pow( pixelCoords[i].x(), 2 );
162  F += std::pow( pixelCoords[i].y(), 2 );
163  G += pixelCoords[i].x() * pixelCoords[i].y();
164  H += pixelCoords[i].x() * mapCoords[i].x();
165  I += pixelCoords[i].y() * mapCoords[i].y();
166  J += pixelCoords[i].x() * mapCoords[i].y();
167  K += mapCoords[i].x() * pixelCoords[i].y();
168  }
169 
170  /* The least squares fit for the parameters { a, b, c, d, x0, y0 } is the
171  solution to the matrix equation Mx = b, where M and b is given below.
172  I *think* that this is correct but I derived it myself late at night.
173  Look at affine.jpg if you suspect bugs. */
174 
175  double MData[] = { A, B, 0, 0, ( double ) n, 0,
176  0, 0, A, B, 0, ( double ) n,
177  E, G, 0, 0, A, 0,
178  G, F, 0, 0, B, 0,
179  0, 0, E, G, 0, A,
180  0, 0, G, F, 0, B
181  };
182 
183  double bData[] = { C, D, H, K, J, I };
184 
185  // we want to solve the equation M*x = b, where x = [a b c d x0 y0]
186  gsl_matrix_view M = gsl_matrix_view_array( MData, 6, 6 );
187  gsl_vector_view b = gsl_vector_view_array( bData, 6 );
188  gsl_vector *x = gsl_vector_alloc( 6 );
189  gsl_permutation *p = gsl_permutation_alloc( 6 );
190  int s;
191  gsl_linalg_LU_decomp( &M.matrix, p, &s );
192  gsl_linalg_LU_solve( &M.matrix, p, &b.vector, x );
193  gsl_permutation_free( p );
194 
195 }
196 #endif
197 
203 void normalizeCoordinates( const QVector<QgsPointXY> &coords, QVector<QgsPointXY> &normalizedCoords,
204  double normalizeMatrix[9], double denormalizeMatrix[9] )
205 {
206  // Calculate center of gravity
207  double cogX = 0.0, cogY = 0.0;
208  for ( int i = 0; i < coords.size(); i++ )
209  {
210  cogX += coords[i].x();
211  cogY += coords[i].y();
212  }
213  cogX *= 1.0 / coords.size();
214  cogY *= 1.0 / coords.size();
215 
216  // Calculate mean distance to origin
217  double meanDist = 0.0;
218  for ( int i = 0; i < coords.size(); i++ )
219  {
220  double X = ( coords[i].x() - cogX );
221  double Y = ( coords[i].y() - cogY );
222  meanDist += std::sqrt( X * X + Y * Y );
223  }
224  meanDist *= 1.0 / coords.size();
225 
226  double OOD = meanDist * M_SQRT1_2;
227  double D = 1.0 / OOD;
228  normalizedCoords.resize( coords.size() );
229  for ( int i = 0; i < coords.size(); i++ )
230  {
231  normalizedCoords[i] = QgsPointXY( ( coords[i].x() - cogX ) * D, ( coords[i].y() - cogY ) * D );
232  }
233 
234  normalizeMatrix[0] = D;
235  normalizeMatrix[1] = 0.0;
236  normalizeMatrix[2] = -cogX * D;
237  normalizeMatrix[3] = 0.0;
238  normalizeMatrix[4] = D;
239  normalizeMatrix[5] = -cogY * D;
240  normalizeMatrix[6] = 0.0;
241  normalizeMatrix[7] = 0.0;
242  normalizeMatrix[8] = 1.0;
243 
244  denormalizeMatrix[0] = OOD;
245  denormalizeMatrix[1] = 0.0;
246  denormalizeMatrix[2] = cogX;
247  denormalizeMatrix[3] = 0.0;
248  denormalizeMatrix[4] = OOD;
249  denormalizeMatrix[5] = cogY;
250  denormalizeMatrix[6] = 0.0;
251  denormalizeMatrix[7] = 0.0;
252  denormalizeMatrix[8] = 1.0;
253 }
254 
255 // Fits a homography to the given corresponding points, and
256 // return it in H (row-major format).
257 void QgsLeastSquares::projective( const QVector<QgsPointXY> &sourceCoordinates,
258  const QVector<QgsPointXY> &destinationCoordinates,
259  double H[9] )
260 {
261 #ifndef HAVE_GSL
262  ( void )sourceCoordinates;
263  ( void )destinationCoordinates;
264  ( void )H;
265  throw QgsNotSupportedException( QStringLiteral( "Calculating a projective transformation requires a QGIS build based GSL" ) );
266 #else
267  Q_ASSERT( sourceCoordinates.size() == destinationCoordinates.size() );
268 
269  if ( destinationCoordinates.size() < 4 )
270  {
271  throw std::domain_error( QObject::tr( "Fitting a projective transform requires at least 4 corresponding points." ).toLocal8Bit().constData() );
272  }
273 
274  QVector<QgsPointXY> sourceCoordinatesNormalized;
275  QVector<QgsPointXY> destinationCoordinatesNormalized;
276 
277  double normSource[9], denormSource[9];
278  double normDest[9], denormDest[9];
279  normalizeCoordinates( sourceCoordinates, sourceCoordinatesNormalized, normSource, denormSource );
280  normalizeCoordinates( destinationCoordinates, destinationCoordinatesNormalized, normDest, denormDest );
281 
282  // GSL does not support a full SVD, so we artificially add a linear dependent row
283  // to the matrix in case the system is underconstrained.
284  uint m = std::max( 9u, ( uint )destinationCoordinatesNormalized.size() * 2u );
285  uint n = 9;
286  gsl_matrix *S = gsl_matrix_alloc( m, n );
287 
288  for ( int i = 0; i < destinationCoordinatesNormalized.size(); i++ )
289  {
290  gsl_matrix_set( S, i * 2, 0, sourceCoordinatesNormalized[i].x() );
291  gsl_matrix_set( S, i * 2, 1, sourceCoordinatesNormalized[i].y() );
292  gsl_matrix_set( S, i * 2, 2, 1.0 );
293 
294  gsl_matrix_set( S, i * 2, 3, 0.0 );
295  gsl_matrix_set( S, i * 2, 4, 0.0 );
296  gsl_matrix_set( S, i * 2, 5, 0.0 );
297 
298  gsl_matrix_set( S, i * 2, 6, -destinationCoordinatesNormalized[i].x()*sourceCoordinatesNormalized[i].x() );
299  gsl_matrix_set( S, i * 2, 7, -destinationCoordinatesNormalized[i].x()*sourceCoordinatesNormalized[i].y() );
300  gsl_matrix_set( S, i * 2, 8, -destinationCoordinatesNormalized[i].x() * 1.0 );
301 
302  gsl_matrix_set( S, i * 2 + 1, 0, 0.0 );
303  gsl_matrix_set( S, i * 2 + 1, 1, 0.0 );
304  gsl_matrix_set( S, i * 2 + 1, 2, 0.0 );
305 
306  gsl_matrix_set( S, i * 2 + 1, 3, sourceCoordinatesNormalized[i].x() );
307  gsl_matrix_set( S, i * 2 + 1, 4, sourceCoordinatesNormalized[i].y() );
308  gsl_matrix_set( S, i * 2 + 1, 5, 1.0 );
309 
310  gsl_matrix_set( S, i * 2 + 1, 6, -destinationCoordinatesNormalized[i].y()*sourceCoordinatesNormalized[i].x() );
311  gsl_matrix_set( S, i * 2 + 1, 7, -destinationCoordinatesNormalized[i].y()*sourceCoordinatesNormalized[i].y() );
312  gsl_matrix_set( S, i * 2 + 1, 8, -destinationCoordinatesNormalized[i].y() * 1.0 );
313  }
314 
315  if ( destinationCoordinatesNormalized.size() == 4 )
316  {
317  // The GSL SVD routine only supports matrices with rows >= columns (m >= n)
318  // Unfortunately, we can't use the SVD of the transpose (i.e. S^T = (U D V^T)^T = V D U^T)
319  // to work around this, because the solution lies in the right nullspace of S, and
320  // gsl only supports a thin SVD of S^T, which does not return these vectors.
321 
322  // HACK: duplicate last row to get a 9x9 equation system
323  for ( int j = 0; j < 9; j++ )
324  {
325  gsl_matrix_set( S, 8, j, gsl_matrix_get( S, 7, j ) );
326  }
327  }
328 
329  // Solve Sh = 0 in the total least squares sense, i.e.
330  // with Sh = min and |h|=1. The solution "h" is given by the
331  // right singular eigenvector of S corresponding, to the smallest
332  // singular value (via SVD).
333  gsl_matrix *V = gsl_matrix_alloc( n, n );
334  gsl_vector *singular_values = gsl_vector_alloc( n );
335  gsl_vector *work = gsl_vector_alloc( n );
336 
337  // V = n x n
338  // U = m x n (thin SVD) U D V^T
339  gsl_linalg_SV_decomp( S, V, singular_values, work );
340 
341  // Columns of V store the right singular vectors of S
342  for ( unsigned int i = 0; i < n; i++ )
343  {
344  H[i] = gsl_matrix_get( V, i, n - 1 );
345  }
346 
347  gsl_matrix *prodMatrix = gsl_matrix_alloc( 3, 3 );
348 
349  gsl_matrix_view Hmatrix = gsl_matrix_view_array( H, 3, 3 );
350  gsl_matrix_view normSourceMatrix = gsl_matrix_view_array( normSource, 3, 3 );
351  gsl_matrix_view denormDestMatrix = gsl_matrix_view_array( denormDest, 3, 3 );
352 
353  // Change coordinate frame of image and pre-image from normalized to destination and source coordinates.
354  // H' = denormalizeMapCoords*H*normalizePixelCoords
355  gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1.0, &Hmatrix.matrix, &normSourceMatrix.matrix, 0.0, prodMatrix );
356  gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1.0, &denormDestMatrix.matrix, prodMatrix, 0.0, &Hmatrix.matrix );
357 
358  gsl_matrix_free( prodMatrix );
359  gsl_matrix_free( S );
360  gsl_matrix_free( V );
361  gsl_vector_free( singular_values );
362  gsl_vector_free( work );
363 #endif
364 }
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.
Definition: qgsexception.h:118
A class to represent a 2D point.
Definition: qgspointxy.h:59
void setX(double x) SIP_HOLDGIL
Sets the x value of the point.
Definition: qgspointxy.h:122
void setY(double y) SIP_HOLDGIL
Sets the y value of the point.
Definition: qgspointxy.h:132
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 ...