georef_tps.c

Go to the documentation of this file.

/****************************************************************************
 *
 * MODULE:       imagery library
 * AUTHOR(S):    Markus Metz
 *
 * PURPOSE:      Image processing library
 * COPYRIGHT:    (C) 2013 by the GRASS Development Team
 *
 *               This program is free software under the GNU General Public
 *               License (>=v2). Read the file COPYING that comes with GRASS
 *               for details.
 *
 *****************************************************************************/
 
#include <stdlib.h>
#include <math.h>
#include <grass/gis.h>
#include <grass/imagery.h>
#include <grass/glocale.h>
#include <signal.h>
 
/* STRUCTURE FOR USE INTERNALLY WITH THESE FUNCTIONS.  THESE FUNCTIONS EXPECT
   SQUARE MATRICES SO ONLY ONE VARIABLE IS GIVEN (N) FOR THE MATRIX SIZE */
 
struct MATRIX {
    int n; /* SIZE OF THIS MATRIX (N x N) */
    double *v;
};
 
/* CALCULATE OFFSET INTO ARRAY BASED ON R/C */
 
#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 **);
static int calcls(struct Control_Points *, struct MATRIX *, double *, double *,
                  double *, double *);
 
static double tps_base_func(const double x1, const double y1, const double x2,
                            const double y2);
static int solvemat(struct MATRIX *, double *, double *, double *, double *);
 
/***********************************************************************
 
  TRANSFORM A SINGLE COORDINATE PAIR.
 
************************************************************************/
 
int I_georef_tps(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 */
                 struct Control_Points *cp, int fwd)
{
    int i, j;
    double dist, *pe, *pn;
 
    if (fwd) {
        pe = cp->e1;
        pn = cp->n1;
    }
    else {
        pe = cp->e2;
        pn = cp->n2;
    }
 
    /* global affine (1st order poly) */
    *e = E[0] + e1 * E[1] + n1 * E[2];
    *n = N[0] + e1 * N[1] + n1 * N[2];
 
    for (i = 0, j = 0; i < cp->count; i++) {
        if (cp->status[i] > 0) {
 
            dist = tps_base_func(e1, n1, pe[i], pn[i]);
 
            *e += E[j + 3] * dist;
            *n += N[j + 3] * dist;
            j++;
        }
    }
 
    return MSUCCESS;
}
 
/***********************************************************************
 
  COMPUTE THE FORWARD AND BACKWARD GEOREFFERENCING COEFFICIENTS
  BASED ON A SET OF CONTROL POINTS
 
************************************************************************/
 
int I_compute_georef_equations_tps(struct Control_Points *cp, double **E12tps,
                                   double **N12tps, double **E21tps,
                                   double **N21tps)
{
    double *tempptr;
    int numactive; /* NUMBER OF ACTIVE CONTROL POINTS */
    int status, i;
    double xmax, xmin, ymax, ymin;
    double delx, dely;
    double xx, yy;
    double sumx, sumy, sumx2, sumy2, sumxy;
    double SSxx, SSyy, SSxy;
 
    /* CALCULATE THE NUMBER OF VALID CONTROL POINTS */
 
    for (i = numactive = 0; i < cp->count; i++) {
        if (cp->status[i] > 0)
            numactive++;
    }
 
    if (numactive < 3)
        return MNPTERR;
 
    if (numactive > 100000) /* arbitrary, admittedly */
        return MNPTERR;
 
    xmin = xmax = cp->e1[0];
    ymin = ymax = cp->n1[0];
 
    sumx = sumy = sumx2 = sumy2 = sumxy = 0.0;
 
    for (i = 0; i < cp->count; i++) {
        if (cp->status[i] > 0) {
            xx = cp->e1[i];
            yy = cp->n1[i];
 
            xmax = MAX(xmax, xx);
            xmin = MIN(xmin, xx);
            ymax = MAX(ymax, yy);
            ymin = MIN(ymin, yy);
 
            sumx += xx;
            sumx2 += xx * xx;
            sumy += yy;
            sumy2 += yy * yy;
            sumxy += xx * yy;
        }
    }
 
    delx = xmax - xmin;
    dely = ymax - ymin;
 
    SSxx = sumx2 - sumx * sumx / numactive;
    SSyy = sumy2 - sumy * sumy / numactive;
    SSxy = sumxy - sumx * sumy / numactive;
 
    if (delx < 0.001 * dely || dely < 0.001 * delx ||
        fabs(SSxy * SSxy / (SSxx * SSyy)) > 0.99) {
        /* points are colinear */
        return MUNSOLVABLE;
    }
 
    xmin = xmax = cp->e2[0];
    ymin = ymax = cp->n2[0];
 
    sumx = sumy = sumx2 = sumy2 = sumxy = 0.0;
    for (i = 0; i < cp->count; i++) {
        if (cp->status[i] > 0) {
            xx = cp->e2[i];
            yy = cp->n2[i];
 
            xmax = MAX(xmax, xx);
            xmin = MIN(xmin, xx);
            ymax = MAX(ymax, yy);
            ymin = MIN(ymin, yy);
 
            sumx += xx;
            sumx2 += xx * xx;
            sumy += yy;
            sumy2 += yy * yy;
            sumxy += xx * yy;
        }
    }
 
    delx = xmax - xmin;
    dely = ymax - ymin;
 
    SSxx = sumx2 - sumx * sumx / numactive;
    SSyy = sumy2 - sumy * sumy / numactive;
    SSxy = sumxy - sumx * sumy / numactive;
 
    if (delx < 0.001 * dely || dely < 0.001 * delx ||
        fabs(SSxy * SSxy / (SSxx * SSyy)) > 0.99) {
        /* points are colinear */
        return MUNSOLVABLE;
    }
 
    /* CALCULATE THE FORWARD TRANSFORMATION COEFFICIENTS */
 
    G_message(_("Calculating forward transformation coefficients"));
    status = calccoef(cp, E12tps, N12tps);
 
    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 */
 
    G_message(_("Calculating backward transformation coefficients"));
    status = calccoef(cp, E21tps, N21tps);
 
    /* 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)
{
    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++;
    }
 
    /* INITIALIZE MATRIX */
 
    m.n = numactive + 3;
 
    m.v = G_calloc(m.n * m.n, sizeof(double));
    if (m.v == NULL)
        G_fatal_error(_("%s: out of memory"),
                      "I_compute_georef_equations_tps()");
    a = G_calloc(m.n, sizeof(double));
    if (a == NULL)
        G_fatal_error(_("%s: out of memory"),
                      "I_compute_georef_equations_tps()");
    b = G_calloc(m.n, sizeof(double));
    if (b == NULL)
        G_fatal_error(_("%s: out of memory"),
                      "I_compute_georef_equations_tps()");
 
    /* equation coefficients */
    *E = G_calloc(m.n, sizeof(double));
    if (*E == NULL)
        G_fatal_error(_("%s: out of memory"),
                      "I_compute_georef_equations_tps()");
    *N = G_calloc(m.n, sizeof(double));
    if (*N == NULL)
        G_fatal_error(_("%s: out of memory"),
                      "I_compute_georef_equations_tps()");
 
    status = calcls(cp, &m, a, b, *E, *N);
 
    G_free(m.v);
    G_free(a);
    G_free(b);
 
    return status;
}
 
/***********************************************************************
 
  CALCULATE THE TRANSFORMATION COEFFICIENTS FOR THIN PLATE SPLINE
  INTERPOLATION.
  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 */
)
{
    int i, j, n, o, numactive = 0;
 
    /* double dist = 0.0, dx, dy, regularization; */
 
    /* INITIALIZE 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;
            if (i != j)
                M(j, i) = 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) {
 
            a[numactive + 3] = cp->e2[n];
            b[numactive + 3] = cp->n2[n];
 
            numactive++;
            M(1, numactive + 3) = 1.0;
            M(2, numactive + 3) = cp->e1[n];
            M(3, numactive + 3) = cp->n1[n];
 
            M(numactive + 3, 1) = 1.0;
            M(numactive + 3, 2) = cp->e1[n];
            M(numactive + 3, 3) = cp->n1[n];
        }
    }
 
    if (numactive < m->n - 3)
        return MINTERR;
 
    i = 0;
    for (n = 0; n < cp->count; n++) {
        if (cp->status[n] > 0) {
            i++;
 
            j = 0;
            for (o = 0; o <= n; o++) {
                if (cp->status[o] > 0) {
                    j++;
                    M(i + 3, j + 3) = tps_base_func(cp->e1[n], cp->n1[n],
                                                    cp->e1[o], cp->n1[o]);
 
                    if (i != j)
                        M(j + 3, i + 3) = M(i + 3, j + 3);
 
                    /* dx = cp->e1[n] - cp->e1[o];
                       dy = cp->n1[n] - cp->n1[o];
                       dist += sqrt(dx * dx + dy * dy); */
                }
            }
        }
    }
 
    /* regularization */
    /* dist /= (numactive * numactive);
       regularization = 0.01 * dist * dist; */
 
    /* set diagonal to regularization, but not the first 3x3 (global affine) */
 
    return solvemat(m, a, b, E, N);
}
 
/***********************************************************************
 
  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[])
{
    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++) {
        G_percent(i - 1, m->n, 4);
        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];
            }
        }
    }
    G_percent(1, 1, 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 MSUCCESS;
}
 
static double tps_base_func(const double x1, const double y1, const double x2,
                            const double y2)
{
    /* official: r * r * log(r) */
    double dist;
 
    if ((x1 == x2) && (y1 == y2))
        return 0.0;
 
    dist = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
 
    return dist * log(dist) * 0.5;
}