From 22f703cab05b7cd368f4de9e03991b7664dc5022 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B6rg=20Frings-F=C3=BCrst?= Date: Mon, 1 Sep 2014 13:56:46 +0200 Subject: Initial import of argyll version 1.5.1-8 --- rspl/stest.c | 654 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 654 insertions(+) create mode 100644 rspl/stest.c (limited to 'rspl/stest.c') diff --git a/rspl/stest.c b/rspl/stest.c new file mode 100644 index 0000000..d1cf2da --- /dev/null +++ b/rspl/stest.c @@ -0,0 +1,654 @@ + +/* + * Scattered Data Interpolation + * with multilevel B-splines + * research. + * + * This is from the paper + * "Scattered Data Interpolation with Multilevel B-Splines" + * by Seungyong Lee, George Wolberg and Sung Yong Shin, + */ + +/* + * Author: Graeme W. Gill + * Date: 2001/1/1 + * + * Copyright 2000 - 2001 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include +#include +#include +#include +#include +#include +#include "../numeric/numlib.h" + + +#ifndef NUMSUP_H +void error(char *fmt, ...), warning(char *fmt, ...); +#endif + +#define MXDI 4 +#define MXDO 4 + +/* A control point */ +typedef struct { + double p[MXDI]; + double v[MXDO]; +} co; + +/* Neighborhood latice cache data */ +typedef struct { + int c[MXDI]; /* Coordinate */ + int xo; /* Offset into srbs latice */ + double w; /* B-spline basis weight */ +} neigh; + +/* Structure that represents a resolution level of B-splines */ +struct _srbs { + struct _mrbs *p; /* Parent structure */ + int res; /* Basic resolution */ + int coi[MXDI]; /* Double increment for each input dimension into latice */ + double *lat; /* Control latice, extending from +/- 1 from 0..res-1 */ + double *_lat; /* Allocation base of lat */ + int lsize, loff; /* Number of doubles in _lat, offset of lat from _lat */ + double w[MXDI]; /* Input data cell width */ + neigh *n; /* Neighborhood latice cache */ + int nsize; /* Number of n entries */ +}; typedef struct _srbs srbs; + +/* Structure that represents the whole scattered interpolation state */ +struct _mrbs { + int di; /* Input dimensions */ + int fdi; /* Output dimensions */ + int tres; /* Target resolution */ + double smf; /* Smoothing factor */ + int npts; /* Number of data points */ + co *pts; /* Coordinate points */ + double l[MXDI], h[MXDI]; /* Input data range, cell width */ + srbs *s; /* Current B-spline latice */ +}; typedef struct _mrbs mrbs; + +static void delete_srbs(srbs *s); + +/* Create a new empty mrbs */ +static mrbs *new_mrbs( +int di, /* Input dimensionality */ +int fdi, /* Output dimesionality */ +int res, /* Target resolution */ +double smf /* Smoothing factor */ +) { + mrbs *p; + if ((p = (mrbs *)malloc(sizeof(mrbs))) == NULL) + error("Malloc mrbs failed"); + + p->di = di; + p->fdi = fdi; + p->tres = res; + p->smf = smf; + p->s = NULL; + + return p; +} + +static void delete_mrbs(mrbs *p) { + + if (p != NULL) { + delete_srbs(p->s); + free(p); + } +} + +/* Create a new empty srbs */ +static srbs *new_srbs( +mrbs *p, /* Parent mrbs */ +int res /* Resolution of this srbs */ +) { + srbs *s; + int e, f; + double *_lat, *lat; /* Latice base address */ + int ix, oe, oo[MXDI]; /* Neighborhood offset index, counter */ + + if ((s = (srbs *)malloc(sizeof(srbs))) == NULL) + error("Malloc srbs failed"); + + s->p = p; + s->res = res; + + for (s->lsize = p->fdi, s->nsize = 1, e = 0; e < p->di; e++) { + s->coi[e] = s->lsize; /* (double) increment in this input dimension */ + s->lsize *= (res + 2); /* Latice in 1D +/- 1 */ + s->nsize *= 4; /* Neighborhood of 4 */ + } + + if ((s->_lat = (double *)malloc(s->lsize * sizeof(double))) == NULL) + error("Malloc srbs latice failed"); + + /* Compute the base address */ + for (s->loff = 0, e = 0; e < p->di; e++) { + s->loff += s->coi[e]; /* Offset by 1 in each input dimension */ + } + s->lat = s->_lat + s->loff; + + /* Figure the cell width */ + for (e = 0; e < p->di; e++) + s->w[e] = (p->h[e] - p->l[e])/(res-1.0); + + /* Setup neighborhood cache info */ + if ((s->n = (neigh *)malloc(s->nsize * sizeof(neigh))) == NULL) + error("Malloc srbs neighborhood failed"); + + for (oe = 0; oe < p->di; oe++) + oo[oe] = 0; + + for(ix = oe = 0; oe < p->di; ix++) { + int xo; + for (xo = e = 0; e < p->di; e++) { + s->n[ix].c[e] = oo[e]; + xo += s->coi[e] * oo[e]; /* Accumulate latice offset */ + } + s->n[ix].xo = xo; + s->n[ix].w = 0.0; + + /* Increment destination offset counter */ + for (oe = 0; oe < p->di; oe++) { + if (++oo[oe] <= 3) /* Counting from 0 ... 3 */ + break; + oo[oe] = 0; + } + } + + return s; +} + +/* Destroy a srbs */ +static void delete_srbs(srbs *s) { + if (s != NULL) { + free(s->_lat); + free(s->n); + free(s); + } +} + +/* Dump the 2D -> 1D contents of an srbs */ +static void dump_srbs(srbs *s) { + int e, f; + int ce, co[MXDI]; /* latice counter */ + mrbs *p = s->p; /* Parent object */ + + /* Init the counter */ + for (ce = 0; ce < p->di; ce++) + co[ce] = -1; + ce = 0; + + f = 0; + while(ce < p->di) { + double v; + int off = 0; /* Latice offset */ + for (e = 0; e < p->di; e++) { + off += co[e] * s->coi[e]; /* Accumulate latice offset */ + } + v = s->lat[off + f]; /* Value of this latice point */ + + printf("Latice at [%d][%d] = %f\n",co[1],co[0],v); + + /* Increment the latice counter */ + for (ce = 0; ce < p->di; ce++) { + if (++co[ce] <= s->res) /* Counting from -1 ... s->res */ + break; + co[ce] = -1; + } + } +} + +/* Initialise an srbs with a linear approximation to the scattered data */ +static void linear_srbs( +srbs *s +) { + int i, e, f; + mrbs *p = s->p; /* Parent object */ + double **A; /* A matrix holding scattered data points */ + double *B; /* B matrix holding RHS & solution */ + + /* Allocate the matricies */ + B = dvector(0, p->npts-1); + A = dmatrix(0, p->npts-1, 0, p->di); + + /* For each output dimension, solve the linear equation coeficients */ + for (f = 0; f < p->fdi; f++) { + int ce, co[MXDI]; /* latice counter */ + + /* Init A[][] with the scattered data points positions */ + /* Also init B[] with the value for this output dimension */ + for (i = 0; i < p->npts; i++) { + for (e = 0; e < p->di; e++) + A[i][e] = p->pts[i].p[e]; + A[i][e] = 1.0; + B[i] = p->pts[i].v[f]; + } + + /* Solve the equation A.x = b using SVD */ + /* (The w[] values are thresholded for best accuracy) */ + /* Return non-zero if no solution found */ + if (svdsolve(A, B, p->npts, p->di+1) != 0) + error("SVD least squares failed"); + /* A[][] will have been changed, and B[] holds the p->di+1 coefficients */ + + /* Use the coefficients to initialise the srbs values */ + for (ce = 0; ce < p->di; ce++) + co[ce] = -1; + ce = 0; + + while(ce < p->di) { + double v = B[p->di]; /* Constant */ + int off = 0; /* Latice offset */ + for (e = 0; e < p->di; e++) { + double lv; + lv = p->l[e] + s->w[e] * co[e]; /* Input value for this latice location */ + v += B[e] * lv; + off += co[e] * s->coi[e]; /* Accumulate latice offset */ + } + s->lat[off + f] = v; /* Value of this latice point */ + + /* Increment the latice counter */ + for (ce = 0; ce < p->di; ce++) { + if (++co[ce] <= s->res) /* Counting from -1 ... s->res */ + break; + co[ce] = -1; + } + } + } + free_dmatrix(A, 0, p->npts-1, 0, p->di); + free_dvector(B, 0, p->npts-1); +} + +/* Do a latice refinement - upsample the current */ +/* source latice to the destination latice. */ +static void refine_srbs( +srbs *ds, /* Destination srbs */ +srbs *ss /* Source srbs */ +) { + mrbs *p = ss->p; /* Parent object */ + int ce, co[MXDI]; /* Source coordinate counter */ + int six; /* Source index */ + int dix; /* destination index */ + static double _wt[5] = { 1.0/8.0, 4.0/8.0, 6.0/8.0, 4.0/8.0, 1.0/8.0 }; + static double *wt = &_wt[2]; /* 1D Distribution weighting */ + + /* Zero the destination latice before accumulating values */ + for (dix = 0; dix < ds->lsize; dix++) + ds->_lat[dix] = 0.0; + + /* Now for each source latice entry, add weighted portions */ + /* to the associated destination points */ + + /* Init the source coordinate counter */ + for (ce = 0; ce < p->di; ce++) + co[ce] = -1; + ce = 0; + six = -ss->loff; + + while(ce < p->di) { + int oe, oo[MXDI]; /* Destination offset counter */ + +//printf("Source coord %d %d, offset %d, value %f\n",co[0], co[1], six, ss->lat[six]); + /* calc destination index, and init offest counter */ + for (dix = oe = 0; oe < p->di; oe++) { + oo[oe] = -2; + dix += co[oe] * 2 * ds->coi[oe]; /* Accumulate dest offset */ + } + oe = 0; + +//printf("Dest coord %d %d\n",co[0] * 2, co[1] * 2); + /* For all the offsets from the destination point */ + while(oe < p->di) { + int e, f, dixo; /* Destination index offset */ + double w = 1.0; /* Weighting */ + +//printf("dest offset %d %d\n",oo[0], oo[1]); + /* Compute dest index offset, and check that we are not outside the destination */ + for (dixo = e = 0; e < p->di; e++) { + int x = co[e] * 2 + oo[e]; /* dest coord */ + dixo += oo[e] * ds->coi[e]; /* Accumulate dest offset */ +//printf("x[%d] = %d\n",e, x); + w *= wt[oo[e]]; /* Compute distribution weighting */ + if (x < -1 || x > ds->res) + break; /* No good */ + } + if (e >= p->di) { /* We are within the destination latice */ +//if ((co[0] * 2 + oo[0]) == 0 && (co[1] * 2 + oo[1]) == 0) { +//printf("Source coord %d %d, offset %d, value %f\n",co[0], co[1], six, ss->lat[six]); +//printf("Dest coord %d %d ix %d, weight %f\n",co[0] * 2 + oo[0], co[1] * 2 + oo[1], dix+dixo, w); +//} + + for (f = 0; f < p->fdi; f++) { /* Distribute weighted values */ + double v = ss->lat[six + f]; +//if ((co[0] * 2 + oo[0]) == 0 && (co[1] * 2 + oo[1]) == 0) +//printf("Value being dist %f, weighted value %f\n", v, v * w); + ds->lat[dix + dixo + f] += v * w; + } + } + + /* Increment destination offset counter */ + for (oe = 0; oe < p->di; oe++) { + if (++oo[oe] <= 2) /* Counting from -2 ... +2 */ + break; + oo[oe] = -2; + } + } + + /* Increment the source index and coordinat counter */ + six += p->fdi; + for (ce = 0; ce < p->di; ce++) { + if (++co[ce] <= ss->res) /* Counting from -1 ... ss->res */ + break; + co[ce] = -1; + } + } +} + +/* Compute the Cubic B-spline weightings for a given t */ +void basis(double b[4], double t) { + double _t3, _t2, _t1, _3t3, _3t2, _3t1, _6t2; + + _t1 = t/6.0; + _t2 = _t1 * _t1; + _t3 = _t2 * _t1; + _3t1 = 3.0 * _t1; + _3t2 = 3.0 * _t2; + _3t3 = 3.0 * _t3; + _6t2 = 6.0 * _t2; + + b[0] = - _t3 + _3t2 - _3t1 + 1.0/6.0; + b[1] = _3t3 - _6t2 + 4.0/6.0; + b[2] = -_3t3 + _3t2 + _3t1 + 1.0/6.0; + b[3] = _t3; +} + + +/* Improve an srbs to make it closer to the scattered data */ +static void improve_srbs( +srbs *s +) { + int i, e, f; + mrbs *p = s->p; /* Parent object */ + double *delta; /* Delta accumulation */ + double *omega; /* Omega accumulation */ + + /* Allocate temporary accumulation arrays */ + if ((delta = (double *)calloc(sizeof(double), s->lsize)) == NULL) + error("Malloc srbs temp latice failed"); + delta += s->loff; + if ((omega = (double *)calloc(sizeof(double), s->lsize)) == NULL) + error("Malloc srbs temp latice failed"); + omega += s->loff; + + /* For each scattered data point */ + for (i = 0; i < p->npts; i++) { + int ix; /* Latice index of base of neighborhood */ + double b[MXDI][4]; /* B-spline basis factors for each dimension */ + double sws; /* Sum of all the basis factors squared */ + double ve[MXDO]; /* Current output value error */ + int nn; /* Neighbor counter */ + + /* Figure out our neighborhood */ + for (ix = e = 0; e < p->di; e++) { + int x; + double t, sp, fp; + sp = (p->pts[i].p[e] - p->l[e])/s->w[e]; /* Scaled position */ + fp = floor(sp); + x = (int)(fp - 1.0); /* Grid coordinate */ + ix += s->coi[e] * x; /* Accume latice offset */ + t = sp - fp; /* Spline parameter */ + basis(b[e], t); /* Compute basis function values */ + } + + /* Compute the grid basis weight functions, */ + /* the sum of the weights squared, and the current */ + /* output value estimate. */ + for (f = 0; f < p->fdi; f++) + ve[f] = p->pts[i].v[f]; /* Target output value */ + for (sws = 0.0, nn = 0; nn < s->nsize; nn++) { + double w; + for (w = 1.0, e = 0; e < p->di; e++) + w *= b[e][s->n[nn].c[e]]; + s->n[nn].w = w; /* cache weighting */ + sws += w * w; + for (f = 0; f < p->fdi; f++) + ve[f] -= w * s->lat[ix + s->n[nn].xo + f]; /* Subtract current aprox value */ + } +printf("Error at point %d = %f\n",i,ve[0]); + + /* Accumulate the delta and omega factors */ + /* for this resolutions improvement. */ + for (nn = 0; nn < s->nsize; nn++) { + double ws, ww, w = s->n[nn].w; + int xo = ix + s->n[nn].xo; /* Latice offset */ + ww = w * w; + ws = ww * w/sws; /* Scale factor for delta */ + omega[xo] += ww; /* Accumulate omega */ + for (f = 0; f < p->fdi; f++) + delta[xo + f] += ws * ve[f]; /* Accumulate delta */ +printf("Distributing omega %f to %d %d\n",ww,s->n[nn].c[0],s->n[nn].c[1]); +printf("Distributing delta %f to %d %d\n",ws * ve[0],s->n[nn].c[0],s->n[nn].c[1]); + } + } + + omega -= s->loff; /* Base them back to -1 corner */ + delta -= s->loff; + + /* Go through the delta and omega arrays, */ + /* compute and add the refinements to the current */ + /* B-spline control latice. */ + for (i = 0; i < s->lsize; i++) { + double om = omega[i]; + if (om != 0.0) { + for (f = 0; f < p->fdi; f++) + s->_lat[i] += delta[i + f]/om; +printf("Adjusting latice index %d by %f to give %f\n",i, delta[i]/om, s->_lat[i]); + } + } + + /* Done with temporary arrays */ + free(omega); + free(delta); +} + +/* Return the interpolated value for a given point */ +/* Return NZ if input point is out of range */ +static int lookup_srbs( +mrbs *p, +co *c /* Point to interpolate */ +) { + srbs *s = p->s; + int e, f; + int ix; /* Latice index of base of neighborhood */ + double b[MXDI][4]; /* B-spline basis factors for each dimension */ + int nn; /* Neighbor counter */ + + /* Figure out our neighborhood */ + for (ix = e = 0; e < p->di; e++) { + int x; + double t, sp, fp; + sp = c->p[e]; + if (sp < p->l[e] || sp > p->h[e]) + return 1; + sp = (sp - p->l[e])/s->w[e]; /* Scaled position */ + fp = floor(sp); + x = (int)(fp - 1.0); /* Grid coordinate */ + ix += s->coi[e] * x; /* Accume latice offset */ + t = sp - fp; /* Spline parameter */ + basis(b[e], t); /* Compute basis function values */ + } + + /* Compute the the current output value. */ + for (f = 0; f < p->fdi; f++) + c->v[f] = 0.0; + for (nn = 0; nn < s->nsize; nn++) { + double w; + for (w = 1.0, e = 0; e < p->di; e++) + w *= b[e][s->n[nn].c[e]]; + for (f = 0; f < p->fdi; f++) + c->v[f] += w * s->lat[ix + s->n[nn].xo + f]; /* Accume spline value */ + } + + return 0; +} + +/* Take a list of scattered data points, */ +/* and setup the mrbs. */ +void set_mrbs( +mrbs *p, /* mrbs to set up */ +co *pts, /* scattered data points (taken) */ +int npts, /* number of scattered data points */ +double *l, /* Input data range, low (May be NULL) */ +double *h /* Input data range, high (May be NULL) */ +) { + int res; + int i, e, f; + srbs *s0 = NULL, *s1; + + /* Establish the input data range */ + for (e = 0; e < p->di; e++) { + if (l == NULL) + p->l[e] = 1e60; + else + p->l[e] = l[e]; + if (h == NULL) + p->h[e] = -1e60; + else + p->h[e] = h[e]; + } + for (i = 0; i < npts; i++) { + for (e = 0; e < p->di; e++) { + if (pts[i].p[e] < p->l[e]) + p->l[e] = pts[i].p[e]; + if (pts[i].p[e] > p->h[e]) + p->h[e] = pts[i].p[e]; + } + } + + /* Take ownership the data */ + p->pts = pts; + p->npts = npts; + + /* Create an initial srbs */ + res = 2; + if ((s1 = new_srbs(p, 2)) == NULL) + error("new_srbs failed"); + + /* Set it up with a linear first approximation */ + linear_srbs(s1); +dump_srbs(s1); + + /* Build up the resolution */ + for (res = 2 * res -1; res < p->tres; res = 2 * res -1) { + +printf("~1 doing resolution %d\n",res); + delete_srbs(s0); + s0 = s1; + if ((s1 = new_srbs(p, res)) == NULL) + error("new_srbs failed"); + + refine_srbs(s1, s0); +dump_srbs(s1); + improve_srbs(s1); + } + + delete_srbs(s0); + p->s = s1; /* Final resolution */ +} + + +/* - - - - - - - - - - - - - - - - */ + +/* Some test points */ +#define NP 5 +co tpts[NP] = { + { { 1, 5 }, { 3 } }, + { { 2, 9 }, { 6 } }, + { { 8, 0 }, { 1 } }, + { { 5, 4 }, { 10 } }, + { { 3, 9 }, { 4 } } +}; + +#define FRES 33 + +int +main(void) { + double ll[MXDI], hh[MXDI]; + mrbs *p; + int i; + +#ifdef NEVER /* Make points be on a plane */ +{ +for (i = 0; i < NP; i++) { + tpts[i].v[0] = 0.5 * tpts[i].p[0] + 0.7 * tpts[i].p[1] + 3.0; +printf("%f, %f -> %f\n", tpts[i].p[0], tpts[i].p[1], tpts[i].v[0]); +} +} +#endif + error_program = "stest"; + + printf("Starting test\n"); + p = new_mrbs(2, 1, FRES, 1.0); + + ll[0] = 0.0; + ll[1] = 0.0; + hh[0] = 10.0; + hh[1] = 10.0; + set_mrbs(p, tpts, NP, ll, hh); + + printf("Interpolation setup\n"); + + /* Check out the accuracy */ + for (i = 0; i < NP; i++) { + co tp; + tp.p[0] = tpts[i].p[0]; + tp.p[1] = tpts[i].p[1]; + if (lookup_srbs(p, &tp)) + printf("Point %d, %f %f outside domain\n",i,tpts[i].p[0],tpts[i].p[1]); + else { + printf("Point %d has value %f, should be %f\n",i,tp.v[0], tpts[i].v[0]); + } + } + + printf("Test done\n"); + return 0; +} + +#ifndef NUMSUP_H +/* Basic printf type error() and warning() routines */ + +void +error(char *fmt, ...) +{ + va_list args; + + fprintf(stderr,"stest: Error - "); + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); + fprintf(stderr, "\n"); + exit (-1); +} + +void +warning(char *fmt, ...) +{ + va_list args; + + fprintf(stderr,"stest: Warning - "); + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); + fprintf(stderr, "\n"); +} + +#endif /* NUMSUP_H */ + -- cgit v1.2.3