move polynomial transformation to imagery lib

git-svn-id: https://svn.osgeo.org/grass/grass/trunk@49845 15284696-431f-4ddb-bdfa-cd5b030d7da7

include/defs/imagery.h

@@ -31,9 +31,9 @@ FILE *I_fopen_subgroup_file_append(const char *, const char *, const char *);
 FILE *I_fopen_subgroup_file_old(const char *, const char *, const char *);
 
 /* georef.c */
-int I_compute_georef_equations(struct Control_Points *, double[3], double[3],
-			       double[3], double[3]);
-int I_georef(double, double, double *, double *, double[3], double[3]);
+int I_compute_georef_equations(struct Control_Points *, double *, double *,
+			       double *, double *, int);
+int I_georef(double, double, double *, double *, double *, double *, int);
 
 /* group.c */
 int I_get_group(char *);

lib/imagery/georef.c

@@ -3,6 +3,11 @@
  *
  * MODULE:       imagery library
  * AUTHOR(S):    Original author(s) name(s) unknown - written by CERL
+ *               Written By: Brian J. Buckley
+ *
+ *               At: The Center for Remote Sensing
+ *               Michigan State University
+ *
  * PURPOSE:      Image processing library
  * COPYRIGHT:    (C) 1999, 2005 by the GRASS Development Team
  *
@@ -12,187 +17,435 @@
  *
  *****************************************************************************/
 
-#include <grass/config.h>
+/*
+ *  Written: 12/19/91
+ *
+ *  Last Update: 12/26/91 Brian J. Buckley
+ *  Last Update:  1/24/92 Brian J. Buckley
+ *  Added printout of trnfile. Triggered by BDEBUG.
+ *  Last Update:  1/27/92 Brian J. Buckley
+ *  Fixed bug so that only the active control points were used.
+ * 
+ */
+
+
+#include <stdlib.h>
+#include <math.h>
+#include <grass/gis.h>
 #include <grass/imagery.h>
 #include <signal.h>
 
-static int floating_exception;
-static void catch(int);
-static double determinant(double, double,
-			  double, double, double, double, double, double,
-			  double);
+/* STRUCTURE FOR USE INTERNALLY WITH THESE FUNCTIONS.  THESE FUNCTIONS EXPECT
+   SQUARE MATRICES SO ONLY ONE VARIABLE IS GIVEN (N) FOR THE MATRIX SIZE */
 
-/* find coefficients A,B,C for e2 = A + B*e1 + C*n1
- * also compute the reverse equations
- *
- * return 0 if no points
- *       -1 if not solvable
- *        1 if ok
- *
- * method is least squares.
- * the least squares problem reduces to solving the following
- * system of equations for A,B,C
- *
- *   s0*A + s1*B + s2*C = x0
- *   s1*A + s3*B + s4*C = x1
- *   s2*A + s4*B + s5*C = x2
- *
- * use Cramer's rule
- *
- *     | x0 s1 s2 |      | s0 x0 s2 |      | s0 s1 x0 |
- *     | x1 s3 s4 |      | s1 x1 s4 |      | s1 s3 x1 |
- *     | x2 s4 s5 |      | s2 x2 s5 |      | s2 s4 x2 |
- * A = ------------  B = ------------  C = ------------ 
- *     | s0 s1 s2 |      | s0 s1 s2 |      | s0 s1 s2 |
- *     | s1 s3 s4 |      | s1 s3 s4 |      | s1 s3 s4 |
- *     | s2 s4 s5 |      | s2 s4 s5 |      | s2 s4 s5 |
- *
- */
+struct MATRIX
+{
+    int n;			/* SIZE OF THIS MATRIX (N x N) */
+    double *v;
+};
+
+/* CALCULATE OFFSET INTO ARRAY BASED ON R/C */
 
-int I_compute_georef_equations(struct Control_Points *cp,
-			       double E12[3], double N12[3], double E21[3],
-			       double N21[3])
+#define M(row,col) m->v[(((row)-1)*(m->n))+(col)-1]
+
+#define MSUCCESS     1		/* SUCCESS */
+#define MNPTERR      0		/* NOT ENOUGH POINTS */
+#define MUNSOLVABLE -1		/* NOT SOLVABLE */
+#define MMEMERR     -2		/* NOT ENOUGH MEMORY */
+#define MPARMERR    -3		/* PARAMETER ERROR */
+#define MINTERR     -4		/* INTERNAL ERROR */
+
+#define MAXORDER 3    /* HIGHEST SUPPORTED ORDER OF TRANSFORMATION */
+
+/***********************************************************************
+
+  FUNCTION PROTOTYPES FOR STATIC (INTERNAL) FUNCTIONS
+
+************************************************************************/
+
+static int calccoef(struct Control_Points *, double *, double *, int);
+static int calcls(struct Control_Points *, struct MATRIX *, double *,
+		  double *, double *, double *);
+static int exactdet(struct Control_Points *, struct MATRIX *, double *,
+		    double *, double *, double *);
+static int solvemat(struct MATRIX *, double *, double *, double *, double *);
+static double term(int, double, double);
+
+/***********************************************************************
+
+  TRANSFORM A SINGLE COORDINATE PAIR.
+
+************************************************************************/
+
+int I_georef(double e1,    /* EASTING TO BE TRANSFORMED */
+	     double n1,	   /* NORTHING TO BE TRANSFORMED */
+	     double *e,	   /* EASTING, TRANSFORMED */
+	     double *n,	   /* NORTHING, TRANSFORMED */
+	     double E[],   /* EASTING COEFFICIENTS */
+	     double N[],   /* NORTHING COEFFICIENTS */
+	     int order	   /* ORDER OF TRANSFORMATION TO BE PERFORMED, MUST MATCH THE
+			      ORDER USED TO CALCULATE THE COEFFICIENTS */
+    )
 {
-    RETSIGTYPE(*sigfpe) (int);
-    double s0, s1, s2, s3, s4, s5;
-    double x0, x1, x2;
-    double det;
-    int i;
-
-
-    s0 = s1 = s2 = s3 = s4 = s5 = 0.0;
-    for (i = 0; i < cp->count; i++) {
-	if (cp->status[i] <= 0)
-	    continue;
-	s0 += 1.0;
-	s1 += cp->e1[i];
-	s2 += cp->n1[i];
-	s3 += cp->e1[i] * cp->e1[i];
-	s4 += cp->e1[i] * cp->n1[i];
-	s5 += cp->n1[i] * cp->n1[i];
-    }
-    if (s0 < 0.5)
-	return 0;
-
-    floating_exception = 0;
-    sigfpe = signal(SIGFPE, catch);
-
-    /* eastings */
-    x0 = x1 = x2 = 0.0;
-    for (i = 0; i < cp->count; i++) {
-	if (cp->status[i] <= 0)
-	    continue;
-	x0 += cp->e2[i];
-	x1 += cp->e1[i] * cp->e2[i];
-	x2 += cp->n1[i] * cp->e2[i];
-    }
+    double e3, e2n, en2, n3, e2, en, n2;
 
-    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
-    if (det == 0.0) {
-	signal(SIGFPE, sigfpe);
-	return -1;
-    }
-    E12[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
-    E12[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
-    E12[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
-
-    /* northings */
-    x0 = x1 = x2 = 0.0;
-    for (i = 0; i < cp->count; i++) {
-	if (cp->status[i] <= 0)
-	    continue;
-	x0 += cp->n2[i];
-	x1 += cp->e1[i] * cp->n2[i];
-	x2 += cp->n1[i] * cp->n2[i];
-    }
+    switch (order) {
+    case 1:
+	*e = E[0] + E[1] * e1 + E[2] * n1;
+	*n = N[0] + N[1] * e1 + N[2] * n1;
+	break;
 
-    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
-    if (det == 0.0) {
-	signal(SIGFPE, sigfpe);
-	return -1;
-    }
-    N12[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
-    N12[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
-    N12[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
-
-    /* the inverse equations */
-
-    s0 = s1 = s2 = s3 = s4 = s5 = 0.0;
-    for (i = 0; i < cp->count; i++) {
-	if (cp->status[i] <= 0)
-	    continue;
-	s0 += 1.0;
-	s1 += cp->e2[i];
-	s2 += cp->n2[i];
-	s3 += cp->e2[i] * cp->e2[i];
-	s4 += cp->e2[i] * cp->n2[i];
-	s5 += cp->n2[i] * cp->n2[i];
-    }
+    case 2:
+	e2 = e1 * e1;
+	n2 = n1 * n1;
+	en = e1 * n1;
 
-    /* eastings */
-    x0 = x1 = x2 = 0.0;
-    for (i = 0; i < cp->count; i++) {
-	if (cp->status[i] <= 0)
-	    continue;
-	x0 += cp->e1[i];
-	x1 += cp->e2[i] * cp->e1[i];
-	x2 += cp->n2[i] * cp->e1[i];
-    }
+	*e = E[0] + E[1] * e1 + E[2] * n1 + E[3] * e2 + E[4] * en + E[5] * n2;
+	*n = N[0] + N[1] * e1 + N[2] * n1 + N[3] * e2 + N[4] * en + N[5] * n2;
+	break;
+
+    case 3:
+	e2 = e1 * e1;
+	en = e1 * n1;
+	n2 = n1 * n1;
+	e3 = e1 * e2;
+	e2n = e2 * n1;
+	en2 = e1 * n2;
+	n3 = n1 * n2;
+
+	*e = E[0] +
+	    E[1] * e1 + E[2] * n1 +
+	    E[3] * e2 + E[4] * en + E[5] * n2 +
+	    E[6] * e3 + E[7] * e2n + E[8] * en2 + E[9] * n3;
+	*n = N[0] +
+	    N[1] * e1 + N[2] * n1 +
+	    N[3] * e2 + N[4] * en + N[5] * n2 +
+	    N[6] * e3 + N[7] * e2n + N[8] * en2 + N[9] * n3;
+	break;
 
-    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
-    if (det == 0.0) {
-	signal(SIGFPE, sigfpe);
-	return -1;
+    default:
+	return MPARMERR;
     }
-    E21[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
-    E21[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
-    E21[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
-
-    /* northings */
-    x0 = x1 = x2 = 0.0;
-    for (i = 0; i < cp->count; i++) {
-	if (cp->status[i] <= 0)
-	    continue;
-	x0 += cp->n1[i];
-	x1 += cp->e2[i] * cp->n1[i];
-	x2 += cp->n2[i] * cp->n1[i];
+
+    return MSUCCESS;
+}
+
+/***********************************************************************
+
+  COMPUTE THE FORWARD AND BACKWARD GEOREFFERENCING COEFFICIENTS
+  BASED ON A SET OF CONTROL POINTS
+
+************************************************************************/
+
+int I_compute_georef_equations(struct Control_Points *cp, double E12[],
+			       double N12[], double E21[], double N21[],
+			       int order)
+{
+    double *tempptr;
+    int status;
+
+    if (order < 1 || order > MAXORDER)
+	return MPARMERR;
+
+    /* CALCULATE THE FORWARD TRANSFORMATION COEFFICIENTS */
+
+    status = calccoef(cp, E12, N12, order);
+
+    if (status != MSUCCESS)
+	return status;
+
+    /* SWITCH THE 1 AND 2 EASTING AND NORTHING ARRAYS */
+
+    tempptr = cp->e1;
+    cp->e1 = cp->e2;
+    cp->e2 = tempptr;
+    tempptr = cp->n1;
+    cp->n1 = cp->n2;
+    cp->n2 = tempptr;
+
+    /* CALCULATE THE BACKWARD TRANSFORMATION COEFFICIENTS */
+
+    status = calccoef(cp, E21, N21, order);
+
+    /* SWITCH THE 1 AND 2 EASTING AND NORTHING ARRAYS BACK */
+
+    tempptr = cp->e1;
+    cp->e1 = cp->e2;
+    cp->e2 = tempptr;
+    tempptr = cp->n1;
+    cp->n1 = cp->n2;
+    cp->n2 = tempptr;
+
+    return status;
+}
+
+/***********************************************************************
+
+  COMPUTE THE GEOREFFERENCING COEFFICIENTS
+  BASED ON A SET OF CONTROL POINTS
+
+************************************************************************/
+
+static int calccoef(struct Control_Points *cp, double E[], double N[],
+		    int order)
+{
+    struct MATRIX m;
+    double *a;
+    double *b;
+    int numactive;		/* NUMBER OF ACTIVE CONTROL POINTS */
+    int status, i;
+
+    /* CALCULATE THE NUMBER OF VALID CONTROL POINTS */
+
+    for (i = numactive = 0; i < cp->count; i++) {
+	if (cp->status[i] > 0)
+	    numactive++;
     }
 
-    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
-    if (det == 0.0) {
-	signal(SIGFPE, sigfpe);
-	return -1;
+    /* CALCULATE THE MINIMUM NUMBER OF CONTROL POINTS NEEDED TO DETERMINE
+       A TRANSFORMATION OF THIS ORDER */
+
+    m.n = ((order + 1) * (order + 2)) / 2;
+
+    if (numactive < m.n)
+	return MNPTERR;
+
+    /* INITIALIZE MATRIX */
+
+    m.v = G_calloc(m.n * m.n, sizeof(double));
+    a = G_calloc(m.n, sizeof(double));
+    b = G_calloc(m.n, sizeof(double));
+
+    if (numactive == m.n)
+	status = exactdet(cp, &m, a, b, E, N);
+    else
+	status = calcls(cp, &m, a, b, E, N);
+
+    G_free(m.v);
+    G_free(a);
+    G_free(b);
+
+    return status;
+}
+
+/***********************************************************************
+
+  CALCULATE THE TRANSFORMATION COEFFICIENTS WITH EXACTLY THE MINIMUM
+  NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.
+
+************************************************************************/
+
+static int exactdet(struct Control_Points *cp, struct MATRIX *m,
+                    double a[], double b[],
+		    double E[],	/* EASTING COEFFICIENTS */
+		    double N[]	/* NORTHING COEFFICIENTS */
+    )
+{
+    int pntnow, currow, j;
+
+    currow = 1;
+    for (pntnow = 0; pntnow < cp->count; pntnow++) {
+	if (cp->status[pntnow] > 0) {
+	    /* POPULATE MATRIX M */
+
+	    for (j = 1; j <= m->n; j++)
+		M(currow, j) = term(j, cp->e1[pntnow], cp->n1[pntnow]);
+
+	    /* POPULATE MATRIX A AND B */
+
+	    a[currow - 1] = cp->e2[pntnow];
+	    b[currow - 1] = cp->n2[pntnow];
+
+	    currow++;
+	}
     }
-    N21[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
-    N21[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
-    N21[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
 
-    signal(SIGFPE, sigfpe);
-    return floating_exception ? -1 : 1;
+    if (currow - 1 != m->n)
+	return MINTERR;
+
+    return solvemat(m, a, b, E, N);
 }
 
-static double determinant(double a, double b, double c, double d, double e,
-			  double f, double g, double h, double i)
+/***********************************************************************
+
+  CALCULATE THE TRANSFORMATION COEFFICIENTS WITH MORE THAN THE MINIMUM
+  NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.  THIS
+  ROUTINE USES THE LEAST SQUARES METHOD TO COMPUTE THE COEFFICIENTS.
+
+************************************************************************/
+
+static int calcls(struct Control_Points *cp, struct MATRIX *m,
+                  double a[], double b[],
+		  double E[],	/* EASTING COEFFICIENTS */
+		  double N[]	/* NORTHING COEFFICIENTS */
+    )
 {
-    /* compute determinant of 3x3 matrix
-     *     | a b c |
-     *     | d e f |
-     *     | g h i |
-     */
-    return a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
+    int i, j, n, numactive = 0;
+
+    /* INITIALIZE THE UPPER HALF OF THE MATRIX AND THE TWO COLUMN VECTORS */
+
+    for (i = 1; i <= m->n; i++) {
+	for (j = i; j <= m->n; j++)
+	    M(i, j) = 0.0;
+	a[i - 1] = b[i - 1] = 0.0;
+    }
+
+    /* SUM THE UPPER HALF OF THE MATRIX AND THE COLUMN VECTORS ACCORDING TO
+       THE LEAST SQUARES METHOD OF SOLVING OVER DETERMINED SYSTEMS */
+
+    for (n = 0; n < cp->count; n++) {
+	if (cp->status[n] > 0) {
+	    numactive++;
+	    for (i = 1; i <= m->n; i++) {
+		for (j = i; j <= m->n; j++)
+		    M(i, j) +=
+			term(i, cp->e1[n], cp->n1[n]) * term(j, cp->e1[n],
+							     cp->n1[n]);
+
+		a[i - 1] += cp->e2[n] * term(i, cp->e1[n], cp->n1[n]);
+		b[i - 1] += cp->n2[n] * term(i, cp->e1[n], cp->n1[n]);
+	    }
+	}
+    }
+
+    if (numactive <= m->n)
+	return MINTERR;
+
+    /* TRANSPOSE VALUES IN UPPER HALF OF M TO OTHER HALF */
+
+    for (i = 2; i <= m->n; i++)
+	for (j = 1; j < i; j++)
+	    M(i, j) = M(j, i);
+
+    return solvemat(m, a, b, E, N);
 }
 
-static void catch(int n)
+/***********************************************************************
+
+  CALCULATE THE X/Y TERM BASED ON THE TERM NUMBER
+
+  ORDER\TERM   1    2    3    4    5    6    7    8    9   10
+  1            e0n0 e1n0 e0n1
+  2            e0n0 e1n0 e0n1 e2n0 e1n1 e0n2
+  3            e0n0 e1n0 e0n1 e2n0 e1n1 e0n2 e3n0 e2n1 e1n2 e0n3
+
+************************************************************************/
+
+static double term(int term, double e, double n)
 {
-    floating_exception = 1;
-    signal(n, catch);
+    switch (term) {
+    case 1:
+	return 1.0;
+    case 2:
+	return e;
+    case 3:
+	return n;
+    case 4:
+	return e * e;
+    case 5:
+	return e * n;
+    case 6:
+	return n * n;
+    case 7:
+	return e * e * e;
+    case 8:
+	return e * e * n;
+    case 9:
+	return e * n * n;
+    case 10:
+	return n * n * n;
+    }
+
+    return 0.0;
 }
 
-int I_georef(double e1, double n1,
-	     double *e2, double *n2, double E[3], double N[3])
+/***********************************************************************
+
+  SOLVE FOR THE 'E' AND 'N' COEFFICIENTS BY USING A SOMEWHAT MODIFIED
+  GAUSSIAN ELIMINATION METHOD.
+
+  | M11 M12 ... M1n | | E0   |   | a0   |
+  | M21 M22 ... M2n | | E1   | = | a1   |
+  |  .   .   .   .  | | .    |   | .    |
+  | Mn1 Mn2 ... Mnn | | En-1 |   | an-1 |
+
+  and
+
+  | M11 M12 ... M1n | | N0   |   | b0   |
+  | M21 M22 ... M2n | | N1   | = | b1   |
+  |  .   .   .   .  | | .    |   | .    |
+  | Mn1 Mn2 ... Mnn | | Nn-1 |   | bn-1 |
+
+************************************************************************/
+
+static int solvemat(struct MATRIX *m, double a[], double b[], double E[],
+		    double N[])
 {
-    *e2 = E[0] + E[1] * e1 + E[2] * n1;
-    *n2 = N[0] + N[1] * e1 + N[2] * n1;
+    int i, j, i2, j2, imark;
+    double factor, temp;
+    double pivot;		/* ACTUAL VALUE OF THE LARGEST PIVOT CANDIDATE */
+
+    for (i = 1; i <= m->n; i++) {
+	j = i;
+
+	/* find row with largest magnitude value for pivot value */
+
+	pivot = M(i, j);
+	imark = i;
+	for (i2 = i + 1; i2 <= m->n; i2++) {
+	    temp = fabs(M(i2, j));
+	    if (temp > fabs(pivot)) {
+		pivot = M(i2, j);
+		imark = i2;
+	    }
+	}
+
+	/* if the pivot is very small then the points are nearly co-linear */
+	/* co-linear points result in an undefined matrix, and nearly */
+	/* co-linear points results in a solution with rounding error */
+
+	if (pivot == 0.0)
+	    return MUNSOLVABLE;
+
+	/* if row with highest pivot is not the current row, switch them */
+
+	if (imark != i) {
+	    for (j2 = 1; j2 <= m->n; j2++) {
+		temp = M(imark, j2);
+		M(imark, j2) = M(i, j2);
+		M(i, j2) = temp;
+	    }
+
+	    temp = a[imark - 1];
+	    a[imark - 1] = a[i - 1];
+	    a[i - 1] = temp;
+
+	    temp = b[imark - 1];
+	    b[imark - 1] = b[i - 1];
+	    b[i - 1] = temp;
+	}
+
+	/* compute zeros above and below the pivot, and compute
+	   values for the rest of the row as well */
+
+	for (i2 = 1; i2 <= m->n; i2++) {
+	    if (i2 != i) {
+		factor = M(i2, j) / pivot;
+		for (j2 = j; j2 <= m->n; j2++)
+		    M(i2, j2) -= factor * M(i, j2);
+		a[i2 - 1] -= factor * a[i - 1];
+		b[i2 - 1] -= factor * b[i - 1];
+	    }
+	}
+    }
+
+    /* SINCE ALL OTHER VALUES IN THE MATRIX ARE ZERO NOW, CALCULATE THE
+       COEFFICIENTS BY DIVIDING THE COLUMN VECTORS BY THE DIAGONAL VALUES. */
+
+    for (i = 1; i <= m->n; i++) {
+	E[i - 1] = a[i - 1] / M(i, i);
+	N[i - 1] = b[i - 1] / M(i, i);
+    }
 
-    return 0;
+    return MSUCCESS;
 }