17
17
#include < stdexcept>
18
18
19
19
#include < gsl/gsl_linalg.h>
20
+ #include < gsl/gsl_blas.h>
20
21
21
22
#include < QObject>
22
23
@@ -174,17 +175,74 @@ void QgsLeastSquares::affine( std::vector<QgsPoint> mapCoords,
174
175
}
175
176
176
177
178
+ /* *
179
+ * Scales the given coordinates so that the center of gravity is at the origin and the mean distance to the origin is sqrt(2).
180
+ *
181
+ * Also returns 3x3 homogenous matrices which can be used to normalize and de-normalize coordinates.
182
+ */
183
+ void normalizeCoordinates ( const std::vector<QgsPoint> coords, std::vector<QgsPoint> &normalizedCoords,
184
+ double normalizeMatrix[9 ], double denormalizeMatrix[9 ])
185
+ {
186
+ // Calculate center of gravity
187
+ double cogX = 0.0 , cogY = 0.0 ;
188
+ for ( uint i = 0 ; i < coords.size (); i++ )
189
+ {
190
+ cogX+= coords[i].x ();
191
+ cogY+= coords[i].y ();
192
+ }
193
+ cogX*= 1.0 /coords.size ();
194
+ cogY*= 1.0 /coords.size ();
195
+
196
+ // Calculate mean distance to origin
197
+ double meanDist = 0.0 ;
198
+ for ( uint i = 0 ; i < coords.size (); i++ )
199
+ {
200
+ double X = (coords[i].x () - cogX);
201
+ double Y = (coords[i].y () - cogY);
202
+ meanDist+= sqrt ( X*X + Y*Y );
203
+ }
204
+ meanDist*= 1.0 /coords.size ();
205
+
206
+ double OOD = meanDist/sqrt (2 );
207
+ double D = 1.0 /OOD;
208
+ normalizedCoords.resize (coords.size ());
209
+ for ( uint i = 0 ; i < coords.size (); i++ )
210
+ {
211
+ normalizedCoords[i] = QgsPoint ( (coords[i].x () - cogX)*D, (coords[i].y () - cogY)*D );
212
+ }
213
+
214
+ normalizeMatrix[0 ] = D; normalizeMatrix[1 ] = 0.0 ; normalizeMatrix[2 ] = -cogX*D;
215
+ normalizeMatrix[3 ] = 0.0 ; normalizeMatrix[4 ] = D; normalizeMatrix[5 ] = -cogY*D;
216
+ normalizeMatrix[6 ] = 0.0 ; normalizeMatrix[7 ] = 0.0 ; normalizeMatrix[8 ] = 1.0 ;
217
+
218
+ denormalizeMatrix[0 ] = OOD; denormalizeMatrix[1 ] = 0.0 ; denormalizeMatrix[2 ] = cogX;
219
+ denormalizeMatrix[3 ] = 0.0 ; denormalizeMatrix[4 ] = OOD; denormalizeMatrix[5 ] = cogY;
220
+ denormalizeMatrix[6 ] = 0.0 ; denormalizeMatrix[7 ] = 0.0 ; denormalizeMatrix[8 ] = 1.0 ;
221
+ }
222
+
177
223
// Fits a homography to the given corresponding points, and
178
224
// return it in H (row-major format).
179
225
void QgsLeastSquares::projective ( std::vector<QgsPoint> mapCoords,
180
226
std::vector<QgsPoint> pixelCoords,
181
227
double H[9 ] )
182
228
{
229
+ Q_ASSERT ( mapCoords.size () == pixelCoords.size () );
230
+
183
231
if ( mapCoords.size () < 4 )
184
232
{
185
233
throw std::domain_error ( QObject::tr ( " Fitting a projective transform requires at least 4 corresponding points." ).toLocal8Bit ().constData () );
186
234
}
187
235
236
+ std::vector<QgsPoint> mapCoordsNormalized;
237
+ std::vector<QgsPoint> pixelCoordsNormalized;
238
+
239
+ double normMap[9 ], denormMap[9 ];
240
+ double normPixel[9 ], denormPixel[9 ];
241
+ normalizeCoordinates ( mapCoords, mapCoordsNormalized, normMap, denormMap );
242
+ normalizeCoordinates ( pixelCoords, pixelCoordsNormalized, normPixel, denormPixel );
243
+ mapCoords = mapCoordsNormalized;
244
+ pixelCoords = pixelCoordsNormalized;
245
+
188
246
// GSL does not support a full SVD, so we artificially add a linear dependent row
189
247
// to the matrix in case the system is underconstrained.
190
248
uint m = std::max ( 9u , ( uint )mapCoords.size () * 2u );
@@ -193,28 +251,28 @@ void QgsLeastSquares::projective( std::vector<QgsPoint> mapCoords,
193
251
194
252
for ( uint i = 0 ; i < mapCoords.size (); i++ )
195
253
{
196
- gsl_matrix_set ( S, i*2 , 0 , pixelCoords[i].x () );
197
- gsl_matrix_set ( S, i*2 , 1 , - pixelCoords[i].y () );
254
+ gsl_matrix_set ( S, i*2 , 0 , pixelCoords[i].x () );
255
+ gsl_matrix_set ( S, i*2 , 1 , pixelCoords[i].y () );
198
256
gsl_matrix_set ( S, i*2 , 2 , 1.0 );
199
257
200
258
gsl_matrix_set ( S, i*2 , 3 , 0.0 );
201
259
gsl_matrix_set ( S, i*2 , 4 , 0.0 );
202
260
gsl_matrix_set ( S, i*2 , 5 , 0.0 );
203
261
204
- gsl_matrix_set ( S, i*2 , 6 , -mapCoords[i].x ()* pixelCoords[i].x () );
205
- gsl_matrix_set ( S, i*2 , 7 , -mapCoords[i].x ()* - pixelCoords[i].y () );
262
+ gsl_matrix_set ( S, i*2 , 6 , -mapCoords[i].x ()*pixelCoords[i].x () );
263
+ gsl_matrix_set ( S, i*2 , 7 , -mapCoords[i].x ()*pixelCoords[i].y () );
206
264
gsl_matrix_set ( S, i*2 , 8 , -mapCoords[i].x ()*1.0 );
207
265
208
266
gsl_matrix_set ( S, i*2 + 1 , 0 , 0.0 );
209
267
gsl_matrix_set ( S, i*2 + 1 , 1 , 0.0 );
210
268
gsl_matrix_set ( S, i*2 + 1 , 2 , 0.0 );
211
269
212
270
gsl_matrix_set ( S, i*2 + 1 , 3 , pixelCoords[i].x () );
213
- gsl_matrix_set ( S, i*2 + 1 , 4 , - pixelCoords[i].y () );
271
+ gsl_matrix_set ( S, i*2 + 1 , 4 , pixelCoords[i].y () );
214
272
gsl_matrix_set ( S, i*2 + 1 , 5 , 1.0 );
215
273
216
- gsl_matrix_set ( S, i*2 + 1 , 6 , -mapCoords[i].y ()* pixelCoords[i].x () );
217
- gsl_matrix_set ( S, i*2 + 1 , 7 , -mapCoords[i].y ()* - pixelCoords[i].y () );
274
+ gsl_matrix_set ( S, i*2 + 1 , 6 , -mapCoords[i].y ()*pixelCoords[i].x () );
275
+ gsl_matrix_set ( S, i*2 + 1 , 7 , -mapCoords[i].y ()*pixelCoords[i].y () );
218
276
gsl_matrix_set ( S, i*2 + 1 , 8 , -mapCoords[i].y ()*1.0 );
219
277
}
220
278
@@ -250,10 +308,20 @@ void QgsLeastSquares::projective( std::vector<QgsPoint> mapCoords,
250
308
H[i] = gsl_matrix_get ( V, i, n - 1 );
251
309
}
252
310
311
+ gsl_matrix *prodMatrix = gsl_matrix_alloc ( 3 , 3 );
312
+
313
+ gsl_matrix_view Hmatrix = gsl_matrix_view_array ( H, 3 , 3 );
314
+ gsl_matrix_view normPixelMatrix = gsl_matrix_view_array ( normPixel, 3 , 3 );
315
+ gsl_matrix_view denormMapMatrix = gsl_matrix_view_array ( denormMap, 3 , 3 );
316
+
317
+ // Change coordinate frame of image and pre-image from normalized to map and pixel coordinates.
318
+ // H' = denormalizeMapCoords*H*normalizePixelCoords
319
+ gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0 , &Hmatrix.matrix , &normPixelMatrix.matrix , 0.0 , prodMatrix );
320
+ gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0 , &denormMapMatrix.matrix , prodMatrix, 0.0 , &Hmatrix.matrix );
321
+
322
+ gsl_matrix_free ( prodMatrix );
253
323
gsl_matrix_free ( S );
254
324
gsl_matrix_free ( V );
255
325
gsl_vector_free ( singular_values );
256
326
gsl_vector_free ( work );
257
327
}
258
-
259
-
0 commit comments