Georeferencer: Normalize coordinates in order to improve numerical conditioning of homography estimation.

git-svn-id: http://svn.osgeo.org/qgis/trunk@13990 c8812cc2-4d05-0410-92ff-de0c093fc19c

Author: mmassing

src/plugins/georeferencer/qgsgeoreftransform.cpp

@@ -562,8 +562,16 @@ bool QgsProjectiveGeorefTransform::updateParametersFromGCPs( const std::vector<Q
 {
   if ( mapCoords.size() < getMinimumGCPCount() )
     return false;
+
+
+  // HACK: flip y coordinates, because georeferencer and gdal use different conventions
+  std::vector<QgsPoint> flippedPixelCoords( pixelCoords.size() );
+  for ( uint i = 0; i < pixelCoords.size(); i++ )
+  {
+    flippedPixelCoords[i] = QgsPoint( pixelCoords[i].x(), -pixelCoords[i].y() );
+  }
 
-  QgsLeastSquares::projective( mapCoords, pixelCoords, mParameters.H );
+  QgsLeastSquares::projective( mapCoords, flippedPixelCoords, mParameters.H );
 
   // Invert the homography matrix using adjoint matrix
   double *H = mParameters.H;

src/plugins/georeferencer/qgsleastsquares.cpp

@@ -17,6 +17,7 @@
 #include <stdexcept>
 
 #include <gsl/gsl_linalg.h>
+#include <gsl/gsl_blas.h>
 
 #include <QObject>
 
@@ -174,17 +175,74 @@ void QgsLeastSquares::affine( std::vector<QgsPoint> mapCoords,
 }
 
 
+/**
+ * Scales the given coordinates so that the center of gravity is at the origin and the mean distance to the origin is sqrt(2).
+ *
+ * Also returns 3x3 homogenous matrices which can be used to normalize and de-normalize coordinates.
+ */
+void normalizeCoordinates( const std::vector<QgsPoint> coords, std::vector<QgsPoint> &normalizedCoords,
+                           double normalizeMatrix[9], double denormalizeMatrix[9])
+{
+  // Calculate center of gravity
+  double cogX = 0.0, cogY = 0.0;
+  for ( uint i = 0; i < coords.size(); i++ )
+  {
+    cogX+= coords[i].x();
+    cogY+= coords[i].y();
+  }
+  cogX*= 1.0/coords.size();
+  cogY*= 1.0/coords.size();
+
+  // Calculate mean distance to origin
+  double meanDist = 0.0;
+  for ( uint i = 0; i < coords.size(); i++ )
+  {
+    double X = (coords[i].x() - cogX);
+    double Y = (coords[i].y() - cogY);
+    meanDist+= sqrt( X*X + Y*Y );
+  }
+  meanDist*= 1.0/coords.size();
+
+  double OOD = meanDist/sqrt(2);
+  double D   = 1.0/OOD;
+  normalizedCoords.resize(coords.size());
+  for ( uint i = 0; i < coords.size(); i++ )
+  {
+    normalizedCoords[i] = QgsPoint( (coords[i].x() - cogX)*D, (coords[i].y() - cogY)*D );
+  }
+
+  normalizeMatrix[0] =   D; normalizeMatrix[1] = 0.0; normalizeMatrix[2] = -cogX*D;
+  normalizeMatrix[3] = 0.0; normalizeMatrix[4] =   D; normalizeMatrix[5] = -cogY*D;
+  normalizeMatrix[6] = 0.0; normalizeMatrix[7] = 0.0; normalizeMatrix[8] =   1.0;
+  
+  denormalizeMatrix[0] = OOD; denormalizeMatrix[1] = 0.0; denormalizeMatrix[2] = cogX;
+  denormalizeMatrix[3] = 0.0; denormalizeMatrix[4] = OOD; denormalizeMatrix[5] = cogY;
+  denormalizeMatrix[6] = 0.0; denormalizeMatrix[7] = 0.0; denormalizeMatrix[8] =  1.0;
+}
+
 // Fits a homography to the given corresponding points, and
 // return it in H (row-major format).
 void QgsLeastSquares::projective( std::vector<QgsPoint> mapCoords,
                                   std::vector<QgsPoint> pixelCoords,
                                   double H[9] )
 {
+  Q_ASSERT( mapCoords.size() == pixelCoords.size() );
+
   if ( mapCoords.size() < 4 )
   {
     throw std::domain_error( QObject::tr( "Fitting a projective transform requires at least 4 corresponding points." ).toLocal8Bit().constData() );
   }
 
+  std::vector<QgsPoint> mapCoordsNormalized;
+  std::vector<QgsPoint> pixelCoordsNormalized;
+  
+  double normMap[9], denormMap[9];
+  double normPixel[9], denormPixel[9];
+  normalizeCoordinates( mapCoords, mapCoordsNormalized, normMap, denormMap );
+  normalizeCoordinates( pixelCoords, pixelCoordsNormalized, normPixel, denormPixel );
+  mapCoords = mapCoordsNormalized;
+  pixelCoords = pixelCoordsNormalized;
+  
   // GSL does not support a full SVD, so we artificially add a linear dependent row
   // to the matrix in case the system is underconstrained.
   uint m = std::max( 9u, ( uint )mapCoords.size() * 2u );
@@ -193,28 +251,28 @@ void QgsLeastSquares::projective( std::vector<QgsPoint> mapCoords,
 
   for ( uint i = 0; i < mapCoords.size(); i++ )
   {
-    gsl_matrix_set( S, i*2, 0,  pixelCoords[i].x() );
-    gsl_matrix_set( S, i*2, 1, -pixelCoords[i].y() );
+    gsl_matrix_set( S, i*2, 0, pixelCoords[i].x() );
+    gsl_matrix_set( S, i*2, 1, pixelCoords[i].y() );
     gsl_matrix_set( S, i*2, 2, 1.0 );
 
     gsl_matrix_set( S, i*2, 3, 0.0 );
     gsl_matrix_set( S, i*2, 4, 0.0 );
     gsl_matrix_set( S, i*2, 5, 0.0 );
 
-    gsl_matrix_set( S, i*2, 6, -mapCoords[i].x()* pixelCoords[i].x() );
-    gsl_matrix_set( S, i*2, 7, -mapCoords[i].x()* -pixelCoords[i].y() );
+    gsl_matrix_set( S, i*2, 6, -mapCoords[i].x()*pixelCoords[i].x() );
+    gsl_matrix_set( S, i*2, 7, -mapCoords[i].x()*pixelCoords[i].y() );
     gsl_matrix_set( S, i*2, 8, -mapCoords[i].x()*1.0 );
 
     gsl_matrix_set( S, i*2 + 1, 0, 0.0 );
     gsl_matrix_set( S, i*2 + 1, 1, 0.0 );
     gsl_matrix_set( S, i*2 + 1, 2, 0.0 );
 
     gsl_matrix_set( S, i*2 + 1, 3, pixelCoords[i].x() );
-    gsl_matrix_set( S, i*2 + 1, 4, -pixelCoords[i].y() );
+    gsl_matrix_set( S, i*2 + 1, 4, pixelCoords[i].y() );
     gsl_matrix_set( S, i*2 + 1, 5, 1.0 );
 
-    gsl_matrix_set( S, i*2 + 1, 6, -mapCoords[i].y()* pixelCoords[i].x() );
-    gsl_matrix_set( S, i*2 + 1, 7, -mapCoords[i].y()* -pixelCoords[i].y() );
+    gsl_matrix_set( S, i*2 + 1, 6, -mapCoords[i].y()*pixelCoords[i].x() );
+    gsl_matrix_set( S, i*2 + 1, 7, -mapCoords[i].y()*pixelCoords[i].y() );
     gsl_matrix_set( S, i*2 + 1, 8, -mapCoords[i].y()*1.0 );
   }
 
@@ -250,10 +308,20 @@ void QgsLeastSquares::projective( std::vector<QgsPoint> mapCoords,
     H[i] = gsl_matrix_get( V, i, n - 1 );
   }
 
+  gsl_matrix *prodMatrix = gsl_matrix_alloc( 3, 3 );
+
+  gsl_matrix_view Hmatrix = gsl_matrix_view_array( H, 3, 3 );
+  gsl_matrix_view normPixelMatrix = gsl_matrix_view_array( normPixel, 3, 3 );
+  gsl_matrix_view denormMapMatrix = gsl_matrix_view_array( denormMap, 3, 3 );
+
+  // Change coordinate frame of image and pre-image from normalized to map and pixel coordinates.
+  // H' = denormalizeMapCoords*H*normalizePixelCoords
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Hmatrix.matrix, &normPixelMatrix.matrix, 0.0, prodMatrix );
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &denormMapMatrix.matrix, prodMatrix, 0.0, &Hmatrix.matrix );
+
+  gsl_matrix_free( prodMatrix );
   gsl_matrix_free( S );
   gsl_matrix_free( V );
   gsl_vector_free( singular_values );
   gsl_vector_free( work );
 }
-
-