/* demo2.c - Allow people to play with network adjustment, with covariances
* © Copyright 2000 by John Halleck, All rights reserved.
*
* This code is in support of the introduction to the subject at
* http://www.cc.utah.edu/~nahaj/cave/survey/intro/
*/
/* Version of May 7th, 2000 */
/* =================== General comments ===================================== */
/* This program allows one to play with network adjustment, given the
* uncertainties of the shots. (Weightings are produced from the
* covariances.)
* It does a full three dimensional weighted least squares solution
* of the LINEARIZED network.
*
* It is a toy program, not intended for serious work.
*
* The program takes in a survey line of the forms:
* frompointnumber topointnumber x y z vx vy vz vxy vxz vyz
* the "#" character starts a comment.
* Shots from point 0 are assumed to tie a point down.
* At least one point must be tied down, no floating sections are allowed.
* So, some examples are:
*
* # Trivial example.
* 0 1 10 10 10 # We pin point one down at (10, 10, 10), with
* # unit covariance
* 1 2 0 0 20 5 # We go straight up, with a variance of 5.
* # in each dimension independently.
* 2 3 0 10 0 2 3 4 # North with different variance for each coordinate.
* # (But coordinates still assumed to be independent)
* 3 4 10 0 0 10 9 8 1 3 2 # East with full 3d covarience matrix.
*
* In addition, instead of a survey line you can use some commands.
* (These aid batch runs.)
* comment data dump text to output.
* echo echo all input.
* end end an individual survey.
* END Assume end of file.
*/
/* ========================== CONFIGURATTION =============================== */
/* This is a toy program, with full (non-sparce) matrix manipulation.
* The techniques for matrix manipulation used are the ones easist
* to understand, but the use MASSIVE amounts of space.
* It makes several matrixes of MAXSHOTS**2
* several of MAXPOINTS**2 and a few of size MAXPOINTS*MAXSHOTS
* If the total size is made too large, then it make the program
* run out of memory.
*/
#define MAXPOINTS 21
#define MAXSHOTS 100
/* ========================== END CONFIGURATTION =========================== */
/* No useful user configuration below this point. */
#include <ctype.h>
/* character test routines such as isdigit() */
#include <stdio.h>
/* We'll have to do IO */
#include <math.h>
/* Just for sqrt */
#include "errors.h"
/* Survey Package standard error values. */
#include "token.h"
/* We will do token scanning */
#include "vec.h"
/* We use the vector routines. */
#include "mat.h"
/* We need the matrix routines. */
#include "survey3d.h"
/* We are surveying in three dimensions. */
#include "survey3debug.h"
/* Routines to dump weight and covariance matrices. */
#include "qinv.h"
/* We will need a matrix inverse */
#include "partition.h"
/* And we make use of matrix partitioning. */
#include "daixyz.h"
/* We check covariance matrices for physical legitimacy */
/* - */
/* This is a toy program, with full (non-sparce) matrix manipulation.
* The techniques for matrix manipulation used are the ones easist
* to understand, but the use MASSIVE amounts of space. Increasing
* the following numbers very far will probably make your program
* run out of memory.
*/
#define MAXPOINTS 21
#define MAXSHOTS 100
/* - */
/* Are we doing multiple surveys? */
int tryonemore = 0;
static double variancetemp[3];
static double weightedsumofsquares; /* What we are, after all, minimizing */
static double expectedsumofsquares; /* What we would expect it to be. */
static double populationvariance;
static double expectedvariance; /* Expected population variance. */
/* Maximum size survey we can handle. */
static int numpoints; /* actual number we have */
static coordinate locations [MAXPOINTS]; /* Final locations */
static double normal [MAXPOINTS*3] [MAXPOINTS*3]; /* Normal equations */
static double solution [MAXPOINTS*3] [MAXPOINTS*3]; /* inverse of normal eqns */
static coordinate rhs [MAXPOINTS]; /* Right hand side of final problem */
/* Maximum number of shots we can handle. */
static int numshots; /* Actual number we have. */
static coordinate shots [MAXSHOTS]; /* shots */
static coordinate weightedshots [MAXSHOTS];
static covariance covariances [MAXSHOTS]; /* The given covariances. */
static weight weights [MAXSHOTS]; /* Weights */
static coordinate newshots [MAXSHOTS]; /* Adjusted shots */
static coordinate residuals [MAXSHOTS]; /* Computed residuals. */
static int frompoint [MAXSHOTS]; /* From and To of shot */
static int topoint [MAXSHOTS];
/* Status to give good non-redundant messages. */
static int havenocovariances = 0; /* Gave "assuming identity" message */
static int haveonecovariance = 0; /* Gave "assuming weight by identity" */
static int havethreecovariances = 0; /* Gave assuming diagonal message */
/* --------------- Deal with standard input -------------------------- */
static int currentline = 1; /* Line number to report errors about. */
static int haveseenEOF = 0; /* We have seen an end of file */
static int echoing = 0; /* Are we echoing the input? */
static error nextfromstdin (int *someplace) {
if (!someplace) return ERRnil;
if (haveseenEOF) return ERRendoffile;
*someplace = getchar();
if (*someplace == EOF) { haveseenEOF = 1; return NoError; }
if (*someplace == '\n') {
currentline++;
if (echoing) printf ("\n ");
} else if (echoing) printf ("%c", *someplace);
return NoError;
}
/* =========================== Initialize data structures. ================= */
static error initialize () {
error problem; /* What problem we've had */
currentline = 1; /* Ready for the first line. */
/* Our arrays start out empty */
numpoints = 0; /* and with a claim that nothing is in them. */
numshots = 0;
if ((problem = matzero (MAXPOINTS*3, MAXPOINTS*3, normal) )) return problem;
if ((problem = matzero (MAXPOINTS*3, MAXPOINTS*3, solution))) return problem;
if ((problem = matzero (3, MAXPOINTS, locations) )) return problem;
if ((problem = matzero (3, MAXSHOTS, shots) )) return problem;
if ((problem = matzero (6, MAXSHOTS, weights) )) return problem;
if ((problem = matzero (6, MAXSHOTS, covariances) )) return problem;
if ((problem = matzero (3, MAXPOINTS, rhs) )) return problem;
if ((problem = matzero (3, MAXSHOTS, weightedshots) )) return problem;
if ((problem = matzero (3, MAXSHOTS, newshots) )) return problem;
if ((problem = matzero (3, MAXSHOTS, residuals) )) return problem;
/* We haven't interacted with the user. */
havenocovariances = 0; /* Haven't given "assuming identity" message */
haveonecovariance = 0; /* Haven't given "assuming variances by identity"
* message */
havethreecovariances = 0; /* Haven't given "assuming diagonal" message */
weightedsumofsquares = 0.0; /* Not known yet. */
expectedsumofsquares = 0.0;
populationvariance = 0.0; /* Not known either. */
expectedvariance = 0.0;
return NoError;
}
/* ========================== User help ==================================== */
static error userhelp (int level) {
printf ("Type in survey in the form:\n");
printf ("From# To# x y z vx vy vz vxy vyz vxz\n");
if (level < 0) return ERRrange;
if (level == 0) return NoError;
printf (" From and To are positive integer point numbers\n");
printf (" point 0 is at the origin. x, y, and z are change between\n");
printf (" this the points and the last. The weights are the elements from\n");
printf (" The covariance matrix.\n");
printf (" This toy program only accepts %d points and %d shots\n",
MAXPOINTS-1, MAXSHOTS);
if (level > 1) {
printf (" Point numbers must not be negative\n");
printf (" and must not have gaps in the numbering.\n");
}
printf ("AT LEAST one point has to be tied down. To tie a point down,\n");
printf ("Just make a shot from point 0 to that point.\n");
printf ("\n");
printf ("From# To# x y z vx vy vz vxy vxz vyz\n");
return NoError;
}
/* =========================== INPUT ======================================= */
#define MAXTOKENSIZE 80
/* --------------- Get an integer from the input ------------------ */
/* We will need to grab integers for the point numbers. */
static error intfrominput (int *result, char *name) {
char buffer[MAXTOKENSIZE];
int status; int sizetoken;
error problem;
if (!result || !name) return ERRnil;
if (!*name) return ERRempty;
if (thischaracter == EOF) return ERRendoffile;
if (thischaracter == '\n') {
printf ("(No %s was provided)\n", name);
return ERRnotfound;
}
if ((problem = skipnoneolwhite()) && problem != ERRnotfound)
return problem;
if ((problem = inttok (MAXTOKENSIZE-2, buffer, &sizetoken))) {
if (ERRnotfound != problem) {
printf ("Unable to read %s\n", name);
printerror (problem);
}
return problem;
}
if (sizetoken >= MAXTOKENSIZE - 2 ) sizetoken = MAXTOKENSIZE - 2;
buffer[sizetoken] = 0;
status = sscanf(buffer,"%d",result);
if (!status) {
printf ("Bad %s \"%s\"\n", name, buffer);
return ERRbaddata;
}
return NoError;
}
/* ------------------ Get a floating point number from the input ------ */
/* We will also need floating point numbers, for coordinates and weights */
static error fltfrominput (double *result, char *name) {
char buffer[MAXTOKENSIZE];
int status; int sizetoken;
error problem;
if (!result || !name) return ERRnil;
if (!*name) return ERRempty;
if (thischaracter == EOF) return ERRendoffile;
if (thischaracter == '\n') {
printf ("No %s was provided\n", name);
return ERRnotfound;
}
if ((problem = skipnoneolwhite()) && problem != ERRnotfound)
return problem;
if ((problem = sflttok (MAXTOKENSIZE-2, buffer, &sizetoken))) {
if (ERRnotfound != problem) {
printf ("Unable to read %s\n", name);
printerror (problem);
}
return problem;
}
if (sizetoken >= MAXTOKENSIZE - 2 ) sizetoken = MAXTOKENSIZE - 2;
buffer[sizetoken] = 0;
status = sscanf(buffer,"%lf",result);
if (!status) {
printf ("Bad %s \"%s\"\n", name, buffer);
return ERRbaddata;
}
return NoError;
}
/* -------------------- Get (possibly degenerate) variances ------- */
/* A comment or end of line is often acceptable... */
static error bailoutifjunk (error problem) {
if (problem != ERRbaddata) return problem;
if ( thischaracter != EOF
&& thischaracter != '\n'
&& thischaracter != '#'
) { skiptoeol(); return problem; }
return skiptoeol();
}
static error grabcovariances(double *vxx, double *vxy, double *vxz,
double *vyy, double *vyz,
double *vzz
) {
error problem;
if (!vxx || !vxy || !vxz || !vyy || !vyz || !vzz) return ERRnil;
/* Set up good defaults. */
*vxx = 1.0; *vxy = 0.0; *vxz = 0.0;
*vyy = 1.0; *vyz = 0.0;
*vzz = 1.0;
if (thischaracter == EOF) return ERRendoffile;
if ((problem = skipnoneolwhite()) && problem != ERRnotfound)
return problem;
/* Do we have anything? */
problem = fltfrominput(vxx, "Variance");
if (problem) {
*vxx = 1.0;
if (problem != ERRnotfound) return problem;
if ((problem = skiptoeol())) return problem;
if (!havenocovariances) {
havenocovariances = 1;
printf ("? Lines without a variance assume:\n");
printf ("? [ 1 0 0 ]\n? [ 0 1 0 ]\n? [ 0 0 1 ]\n");
}
return bailoutifjunk(NoError);
}
if ((problem = fltfrominput(vyy, "Y Variance"))) {
*vyy = *vxx; *vzz = *vxx;
if (problem != ERRnotfound) return problem;
if ((problem = skiptoeol())) return problem;
if (!haveonecovariance) {
haveonecovariance = 1;
printf ("? Lines with a single variance are assumed to be\n");
printf ("? that value multiplied by the identity matrix\n");
printf ("? In this case it would be:\n");
printf ("? [ %f %f %f ]\n", *vxx, 0.0, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, *vxx, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, 0.0, *vxx);
}
return bailoutifjunk(NoError);
}
if ((problem = fltfrominput(vzz, "Z variance"))) {
*vzz = 1.0;
if (problem == ERRnotfound) return bailoutifjunk(ERRbaddata);
return problem;
}
/* Ok, Now let's try for the covariances */
if ((problem = fltfrominput(vxy, "Covariance between x and y"))) {
*vxy = 0.0;
if (problem != ERRnotfound) return problem;
if ((problem = skiptoeol())) return problem;
if (!havethreecovariances) {
havethreecovariances = 1;
printf ("? Lines without covariances will be assumed to be\n");
printf ("? diagonal matrices. In this case:\n");
printf ("? [ %f %f %f ]\n", *vxx, 0.0, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, *vyy, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, 0.0, *vzz);
}
return bailoutifjunk(NoError);
}
/* Ok, we have one, let's get the rest. */
if ((problem = fltfrominput(vxz, "Covariance between X and Z"))) {
*vxz = 0.0;
if (problem == ERRnotfound) return bailoutifjunk(ERRbaddata);
return problem;
}
if ((problem = fltfrominput(vyz, "Covariance of relation between y and z"))) {
*vyz = 1.0;
if (problem == ERRnotfound) return bailoutifjunk(ERRbaddata);
return problem;
}
if ((problem = skipnoneolwhite())) return problem;
if (thischaracter != EOF && thischaracter != '\n' && thischaracter != '#') {
printf ("*** This line contains junk after the valid data.\n");
}
return bailoutifjunk(NoError);
}
/* -------------------- Bail out ------------------------------ */
/* Bail out, reporting to user */
static error bailout(error problem) {
printf ("*** Line not accepted\n");
if (problem != ERRnotfound && problem != ERRbaddata) return problem;
if ((problem = skipeol())) return problem;
return NoError;
}
/* ------------------ Process a command -------------------------------- */
static error processcommand () {
error problem;
char buffer[MAXTOKENSIZE]; int returnedsize;
/* Commands are "?" for help, and "comment" to echo input. */
/* This can't be a survey line, maybe it is a command? */
if ((problem = nonwhitetok(MAXTOKENSIZE-2, buffer, &returnedsize)))
return problem;
if (returnedsize == 0) return ERRnotfound;
if (returnedsize > MAXTOKENSIZE-2) returnedsize = MAXTOKENSIZE-2;
buffer[returnedsize] = 0; /* terminate token properly for C call */
if (returnedsize==1) {
if (buffer[0] == '?') return userhelp(2);
else if (buffer[0]=='h') return userhelp(2);
} else if (returnedsize == 4) {
if (buffer[0]=='h' && buffer[1]=='e' && buffer[2]=='l' && buffer[3]=='p')
return userhelp(2);
if (buffer[0]=='e' && buffer[1]=='c' && buffer[2]=='h' && buffer[3]=='o')
{ echoing = 1; printf ("\n "); return NoError; }
} else if (returnedsize == 7) {
if (buffer[0]=='c' && buffer[1]=='o' && buffer[2]=='m' && buffer[3]=='m'
&& buffer[4]=='e' && buffer[5]=='n' && buffer[6]=='t'
){
printf ("--- COMMENT: ");
while (thischaracter != EOF && thischaracter != '\n') {
if (!echoing) printf ("%c", thischaracter);
/* If we are already echoing, then it's already been echoed. */
if ((problem = nextchar())) return problem;
}
printf ("\n");
return NoError;
}
} else if ( returnedsize == 6
&& buffer[0]=='n' && buffer[1]=='o' && buffer[2]=='e'
&& buffer[3]=='c' && buffer[4]=='h' && buffer[5]=='o'
) {
echoing = 0;
return NoError;
} else if ( returnedsize == 3 ) {
if ( buffer[0]=='e' && buffer[1]=='n' && buffer[2]=='d') {
printf ("\n--- SURVEY TERMINATED --\n\n");
tryonemore = 1; /* There may be another survey following */
return ERRendoffile;
} else if ( buffer[0]=='E' && buffer[1]=='N' && buffer[2]=='D' ) {
printf ("\n---- INPUT TERMINATED --\n\n");
tryonemore = 0;
return ERRnotfound;
}
}
printf ("Unknown command \"%s\" on line.\n", buffer);
return bailout(ERRbaddata);
}
/* ------------------ Process the line. ------------------------------- */
static error processline() {
error problem; /* Status of support routines. */
/* returns the error if unexpected errors seen, or if errors would prevent
* the program from continuing. Otherwise it returns NoError or
* ERRnotfound.
*/
int from, to; /* Stations for from and two */
double x, y, z; /* Coordinates */
double vxx, vxy, vxz,
vyy, vyz,
vzz; /* Covariances. */
double wxx, wxy, wxz,
wyy, wyz,
wzz; /* Weights */
/* Skip white space and comments */
while ( thischaracter != EOF
&& (thischaracter == '#' || isspace(thischaracter))
) {
if (thischaracter == '#') { if ((problem = skipeol())) return problem; }
if (ERRnotfound != (problem = skipwhite())) return problem;
}
if (thischaracter == EOF) return ERRendoffile;
/* What do we have? */
if (!isdigit(thischaracter)) return processcommand();
/* This can't be a survey line, maybe it is a command? */
/* OK, we must now have a shot line to play with... */
/* FROM */
if ((problem = intfrominput(&from, "\"FROM\" station")))
return bailout(problem);
if (from >= MAXPOINTS) {
if ((problem = skiptoeol())) return problem;
printf ("? from point number too large, max is %d\n", MAXPOINTS-1);
return bailout(NoError);
}
/* TO */
if ((problem = intfrominput (&to, "\"TO\" station")))
return bailout(problem);
if (from >= MAXPOINTS) {
if ((problem = skiptoeol())) return problem;
printf ("??? from point number too large, max is %d\n", MAXPOINTS-1);
return bailout(NoError);
}
if (to == from) {
if ((problem = skiptoeol())) return problem;
printf ("??? It is not possible for a shot to be from and to the ");
printf ("SAME point\n");
return bailout(NoError);
}
/* X */
if ((problem = fltfrominput (&x, "X coordinate"))) return bailout(problem);
/* Y */
if ((problem = fltfrominput (&y, "Y coordinate"))) return bailout(problem);
/* Z */
if ((problem = fltfrominput (&z, "Z coordinate"))) return bailout(problem);
if (from != 0 && to != 0 && x == 0.0 && y == 0.0 && z == 0.0) {
if ((problem = skiptoeol())) return problem;
printf ("??? Length of shot can't be zero\n");
return bailout(NoError);
}
if (numshots >= MAXSHOTS) {
printf ("*** Too many shots for this toy program to deal with.\n");
printf ("*** Max is %d\n", MAXSHOTS);
return bailout(ERRoverflow);
}
/* Get what the user is willing to give for a weight */
if ((problem = grabcovariances (&vxx, &vxy, &vxz, &vyy, &vyz, &vzz))) {
if (problem == ERRnotfound || problem == ERRbaddata) return problem;
return bailout(problem);
}
/* Read the rest of the line, so it gets echoed completely. */
if ((problem = skiptoeol())) return problem;
if ((problem = loadcvr (covariances[numshots], vxx, vxy, vxz, vyy, vyz, vzz)))
return problem;
printf ("Echo of shot %d: From point %d, To point %d,\n",
numshots+1, from, to);
printf ("Change in (X,Y,Z)= (%f %f %f)\n", x, y, z);
if ((problem = loadcvr (covariances[numshots], vxx, vxy, vxz, vyy, vyz, vzz)))
return problem;
if ( (problem = covarprint ("Covariance matrix",
covariances[numshots])) ) return problem;
if ( (problem = covarok (covariances[numshots])) ) {
if (problem == ERRfalse) {
return bailout(NoError);
}
return problem;
}
if ( (problem = cvr2wht (weights[numshots], covariances[numshots])) ) {
if (problem == ERRnumeric) {
printf ("? Covariance matrix doesn't represent a physically\n");
printf ("? possible situation. [SINGULAR]");
return bailout(NoError);
}
return problem;
}
if ( (problem = weightprint ("Computed weight matrix", weights[numshots])) )
return problem;
wxx = weights[numshots][0];
wxy = weights[numshots][1];
wxz = weights[numshots][2];
wyy = weights[numshots][3];
wyz = weights[numshots][4];
wzz = weights[numshots][5];
if (wxy > (wxx+wyy)/2.0) {
printf ("? Matrix not Positive Definite Wxy>(Wxx+Wyy)/2)\n");
printf ("? %f > (%f + %f) / 2\n", wxy, wxx, wyy);
return bailout(NoError);
}
if (wyz > (wyy+wzz)/2.0) {
printf ("? Matrix not Positive Definite Wyz>(Wyy+Wzz)/2\n");
printf ("? %f > (%f + %f) / 2\n", wyz, wyy, wzz);
return bailout(NoError);
}
if (wxz > (wxx+wzz)/2.0) {
printf ("? Matrix not Positive Definite Wxz>(Wxx+Wzz)/2\n");
printf ("? %f > (%f + %f) / 2\n", wxz, wxx, wzz);
return bailout(NoError);
}
shots[numshots][0] = x; shots[numshots][1] = y; shots[numshots][2] = z;
frompoint[numshots] = from; topoint[numshots] = to;
numshots++; /* *NOW* AFTER we know we have no problems, note new shot. */
return NoError;
}
/* ------------------ Get another input line. -------------------------- */
static error getinput () {
error problem;
while (thischaracter != EOF) {
if ((problem = processline())) {
if (problem == ERRendoffile) return NoError;
else return problem;
}
if ((problem = skipeol()) && problem != ERRnotfound) return problem;
}
return NoError;
}
/* =========================== Echo Data =================================== */
static error echoit () {
int i;
if (numshots < 1) return ERRempty;
printf ("\n\nThe Survey we are processing has %d shots, and is as follows:\n",
numshots);
for (i=0; i<numshots; i++) {
printf ("%d %d ", frompoint[i], topoint[i]);
printf ("%f %f %f ", shots[i][0], shots[i][1], shots[i][2]);
printf ("%f %f %f %f %f %f", covariances[i][0], covariances[i][3],
covariances[i][5], covariances[i][1],
covariances[i][2], covariances[i][4]);
printf ("\n");
}
printf ("\n");
return NoError;
}
/* =========================== Dump debug of normal equations ============== */
/* DEBUG CODE
static error dumpeqn (char *text, int rows, int cols, double *which) {
int i, j;
if (!which) return ERRnil;
if (!text) return ERRnil;
if (!*text) return ERRempty;
if (rows<1 || cols<1) return ERRsize;
printf ("DEBUG: %s\n", text);
for (i=0; i<rows; i++) {
printf ("[");
for (j=0; j<cols; j++) {
printf (" %f", *which++);
}
printf (" ]\n");
}
printf ("\n");
return NoError;
}
END DEBUG */
/* =========================== Prep it ===================================== */
static error prepit () {
/* We very seriously assume that consistency checking was done and passed */
int from, to, temp;
int whichshot;
error problem;
double shotweight[3][3];
/* Now we need to form the normal equations.
* This would be transpose(A)*weights*A
* but we know that if we look at matrix multiplication as
* outer products, it is easy to prove that any row of A will
* affect only four elements of the normal equations. If
* the shot is from point i to point j, then the elements
* affected are N(i,i), N(j,j), N(i,j), N(j,i).
* The weight will be added to the diagonal elements, and subtracted
* from the off diagonal elements.
* So... We will just do that directly.
* In addition, any point k pinned down will only affect element N(k,k)
* of the normal equations, and will affect them only by adding the
* weight.
*/
for (whichshot=0; whichshot<numshots; whichshot++) {
from = frompoint[whichshot]; to = topoint[whichshot];
if (to == 0 && from != 0) {
/* Opps, backwards pin, lets turn it around. */
temp = to; to = from; from = temp;
shots [whichshot][0] = - shots [whichshot][0];
shots [whichshot][1] = - shots [whichshot][1];
shots [whichshot][2] = - shots [whichshot][2];
}
if ((problem = matfmwht (shotweight, weights[whichshot])))
return problem;
if (from == 0) { /* Just a pin */
if ((problem = partadd (numpoints*3, numpoints*3, normal,
3, 3, shotweight,
(to-1)*3,(to-1)*3))) return problem;
} else { /* A full real shot. */
/* We need to add this to the diagonal elements. */
if ((problem = partadd (numpoints*3, numpoints*3, normal,
3, 3, shotweight,
(from-1)*3,(from-1)*3))) return problem;
if ((problem = partadd (numpoints*3, numpoints*3, normal,
3, 3, shotweight,
(to-1)*3,(to-1)*3))) return problem;
/* And we need to subtract this from the off diagonal elements. */
if ((problem = partsub (numpoints*3, numpoints*3, normal,
3, 3, shotweight,
(from-1)*3,(to-1)*3))) return problem;
if ((problem = partsub (numpoints*3, numpoints*3, normal,
3, 3, shotweight,
(to-1)*3,(from-1)*3))) return problem;
}
}
/* DEBUG:
dumpeqn ("Normal equations after forming.",
numpoints*3, numpoints*3, (double *)&normal);
END DEBUG */
/* We've now formed the Normal equations which we are going to invert.
* We also have to form transpose(A)*weights*shots for the right
* hand side. We will do this in two steps, first forming weights*shots
* And then forming the product.
*/
for (whichshot=0; whichshot<numshots; whichshot++) {
if ((problem = wxcmult (weightedshots[whichshot],
weights[whichshot], shots[whichshot])
))
return problem;
}
/* DEBUG:
dumpeqn ("Weighted shots", numshots, 3, (double *)&weightedshots);
END DEBUG */
/* We can now form the right hand side. */
for (whichshot=0; whichshot<numshots; whichshot++) {
from = frompoint[whichshot]; to = topoint[whichshot];
if ((problem = addcoord (rhs[to-1], rhs[to-1], weightedshots[whichshot])))
return problem;
if (from != 0) {
if ((problem = subcoord (rhs[from-1], rhs[from-1],
weightedshots[whichshot])))
return problem;
}
}
/* DEBUG:
dumpeqn ("Right hand side", numpoints, 3, (double *)&rhs);
END DEBUG: */
return NoError;
}
/* =========================== CONSISTENCY CHECKING ======================== */
static error checkit () {
int i;
int seen[MAXPOINTS];
int reachable[MAXPOINTS];
int seentie = 0;
int moretodo;
int to, from;
int floating;
if (numshots < 1) return ERRempty;
for (i=0; i<MAXPOINTS; i++) { seen[i] = 0; reachable[i] = 0;}
/* We assume consecutive point numbers here. If there are gaps, then
* there are going to be zero rows or columns in the final problem
* matrix.
*/
/* Find what points we have */
for (i=0; i<numshots; i++) {
if (frompoint[i] > numpoints) numpoints = frompoint[i];
if (topoint [i] > numpoints) numpoints = topoint[i];
seen[frompoint[i]] = 1;
seen[topoint [i]] = 1;
if (frompoint[i] == topoint[i]) {
printf ("??? A shot can't be from and to the same point.\n");
return ERRbaddata;
}
if (frompoint[i] == 0 || topoint[i] == 0) seentie = 1;
}
/* In order for the problem to be non-singular, AT LEAST ONE point must
* be tied down. We noted above each time we saw a point tied down.
* So, if we didn't see a tie, die.
*/
if (!seentie) {
printf ("???Entire survey appears to be floating. No points tied down.");
return ERRbaddata;
}
/* It is *critical* that the problem have no floating shots. */
/* This is, by far, not the best way to check this, but it will do for
* a toy program.
*/
if (numshots < numpoints) {
printf ("??? There are fewer survey shots than point numbers used...\n");
printf ("??? Assuming floating sections, bailing out.\n");
return ERRbaddata;
}
/* We marked all the points we have seen above, so if we find that a point
* in the range that isn't marked, we know we have problems.
*/
for (i=0; i<numpoints; i++) {
if (!seen[i]) {
printf ("Max point number was: %d, point %d not found.\n",
numpoints, i);
return ERRbaddata;
}
}
/* It is *critical* that the problem have no floating shots. */
/* This is, by far, not the best way to check this, but it will do for
* a toy program.
*/
moretodo = 1;
reachable[0]=1;
while (moretodo) {
moretodo = 0;
for (i=0; i<numshots; i++) {
to = topoint[i]; from = frompoint[i];
if (reachable[from] && !reachable[to]) {
reachable[to] = 1; moretodo = 1;
} else if (reachable[to] && !reachable[from]) {
reachable[from] = 1; moretodo = 1;
}
}
}
floating = 0;
for (i=0; i<numpoints; i++) if (!reachable[i]) {
printf ("??? Point %d is in a floating section.\n", i);
floating = 1;
}
if (floating) {
printf ("??? Processing can not continue with floating sections.\n");
return ERRbaddata;
}
return NoError;
}
/* =========================== OUTPUT ====================================== */
static error sparsity (char *kind, matrix given) {
int i, j; /* loop indices */
double *thisone;
if (!kind) return ERRnil;
if (!given) return ERRnil;
if (!*kind) return ERRempty;
if (!numpoints) {
printf ("%s matrix is empty.\n", kind);
return ERRempty;
} else {
printf ("%s sparsity pattern:\n", kind);
thisone = (double *) given;
for (i=0; i<numpoints*3; i++) {
printf ("[ ");
for (j=0; j<numpoints*3; j++) {
if (*thisone == 0.0) printf (".");
else if (*thisone > 0.0) printf ("+");
else printf ("-");
if ((j%3==2) && j<numpoints*3-1) printf (" ");
thisone++;
}
printf (" ]\n");
if ((i%3==2) && i<numpoints*3-1) {
printf ("[ ");
for (j=0; j<numpoints*3; j++) {
printf (" ");
if ((j%3==2) && j<numpoints*3-1) printf (" ");
}
printf (" ]\n");
}
}
printf ("\n");
}
return NoError;
}
static error outpoints (int size, char *label, double *data) {
int i; /* Loop index */
if (!label) return ERRnil;
if (!*label) return ERRempty;
if (size < 0) return ERRrange;
printf ("%s:\n", label);
if (size == 0) {
printf ("No points\n");
return ERRempty;
}
for (i=0; i<size; i++) {
printf ("%3d: [ %f %f %f ]'\n", i+1, *data++, *data++, *data++);
}
return NoError;
}
static error output() {
error problem;
if (!numpoints)
return ERRempty;
if ((problem = sparsity ("Solution", solution)))
return problem;
if ((problem = outpoints(numshots, "Residuals", (double *) &residuals)))
return problem;
printf ("Weighted sum of the squares = %f\n", weightedsumofsquares);
printf ("(Expected maximum sum of squares = %f)\n", expectedsumofsquares);
if (numshots == numpoints) {
printf ("This survey had no redundancy... Therefore no detectable ");
printf ("errors to adjust.\n");
} else {
printf ("Estimated weighted population variance = %f\n",
populationvariance);
printf ("(Expected variance = %f)\n", expectedvariance);
printf ("(Expected weighted standard deviation of error %f)\n",
sqrt(expectedvariance));
printf ("Estimated weighted standard deviation of error: %f\n",
sqrt(populationvariance));
printf ("Degrees of freedom of this survey = %d\n", numshots-numpoints);
}
printf ("\n");
if ((problem = outpoints(numpoints,
"Final solution in terms of points",
(double *) &locations))
) return problem;
if ((problem = outpoints(numshots,
"Final solution in terms of shots",
(double *) &newshots))
) return problem;
return NoError;
}
/* =========================== Actual solve ================================ */
static error solve () {
error problem;
int i;
coordinate temp;
double product;
if (numpoints < 1) return ERRempty;
/* We have the problem, A points = shots
* Which we want to solve with weighting.
* That gives us A'WA points = A'W shots.
* A'WA is in the array "normal".
* A'W shots is in the array rhs.
* So... Now we just invert the normal equations....
*/
if ((problem = invpd (numpoints*3, solution, normal, normal)))
return problem;
/* And multiply the right hand side by that inverse, giving the points. */
if ((problem = matmult (
numpoints*3, 1, locations,
numpoints*3, solution, rhs
)
)) return problem;
/* By most cave survey standards, this would be the end...
* but for any analysis at all we'll need the residuals.
*/
/* A' by locations is the new shots, and new shots - old shots
* is the residual (Normally indicated by "v" in the literature).
* Of course, we never actually formed A', so we'll have to fake
* the multiply here from the shot list.
*/
/* First, compute up the new shots */
/* Start fresh */
if ((problem = matzero (3, numshots, newshots))) return problem;
if ((problem = matzero (3, numshots, residuals))) return problem;
for (i=0; i<numshots; i++) { /* For each shot */
if (frompoint[i] != 0) {
if ((problem = subcoord ( newshots[i], newshots[i],
locations[frompoint[i]-1]))
) return problem;
}
if ((problem = addcoord(newshots[i], newshots[i],
locations[topoint[i]-1]))
) return problem;
}
/* Then turn that into residuals. */
for (i=0; i<numshots; i++) {
if ((problem = vecsub (3, residuals[i], newshots[i], shots[i])))
return problem;
}
/* Once that's done. then we can find the weighted sum of the squares
* of the residual.
* [ That is, after all, what the least squares solution is
* minimizing...]
* What? Your favorite adjustment program doesn't even compute
* the number it is minimizing? For shame.
*/
weightedsumofsquares = 0.0;
for (i=0; i<numshots; i++) {
if ((problem = wxcmult (temp, weights[i], residuals[i])))
return problem;
if ((problem = vecdot (3, &product, temp, residuals[i])))
return problem;
weightedsumofsquares += product;
}
expectedsumofsquares = 0.0;
for (i=0; i<numshots; i++){
variancetemp[0] = covariances[i][0];
variancetemp[1] = covariances[i][3];
variancetemp[2] = covariances[i][5];
/* printf ("DEBUG: vartemp[%i] = [%f, %f, %f]'\n", i,
variancetemp[0], variancetemp[1], variancetemp[2]);
*/
if ((problem = wxcmult (temp, weights[i], variancetemp)))
return problem;
if ((problem = vecdot (3, &product, temp, variancetemp)))
return problem;
expectedsumofsquares += product;
}
/* printf ("DEBUG: Final expected sum: %f\n", expectedsumofsquares); */
/* Population variance is handy */
/* We probably ought to compute the aposteriori reference variance also. */
populationvariance = weightedsumofsquares / (3.0*numshots);
expectedvariance = expectedsumofsquares / (3.0*numshots);
return NoError;
}
/* ========================================================================= */
/* ======================= Process a survey =============================== */
/* ========================================================================= */
static error processasurvey () {
error problem; /* What problems did the support stuff see? */
tryonemore = 0; /* We will by default assume this is the last survey */
echoing = 0; /* Don't echo data unless asked for */
if ((problem = initialize()))
aborterror (problem, "Couldn't initialize data structures");
if ((problem = tokinitialize(&nextfromstdin)))
aborterror (problem, "Problem opening token scanning package.");
if ((problem = getinput())) {
tokfinalize(); /* We don't care if this worked */
/* If this was an end of file forced by an END, just bail */
if (ERRnotfound == problem) return ERRendoffile;
aborterror (problem, "Couldn't get the input.");
}
if ((problem = tokfinalize()))
aborterror (problem, "Problem closing token scanning package.");
printf ("--- Survey input completed ---\n");
if ((problem = echoit())) {
if (problem == ERRempty) {
printf ("*** Survey had no valid shots\n");
exit (ERRempty);
} else aborterror (problem, "Survey couldn't be printed back out?");
}
printf ("--- Echoing of input completed ---\n");
if ((problem = checkit())) {
if (problem == ERRempty) {
printf ("***\a No valid survey was given\n");
exit (ERRempty);
} else if (problem == ERRbaddata) {
printf ("***\a Survey not processable\n");
exit (ERRbaddata);
} else aborterror (problem, "Problem couldn't be checked.");
}
printf ("--- Consistency checked ---\n");
if ((problem = prepit())) {
aborterror (problem, "Couldn't prepare the problem");
}
printf ("--- Problem prepared ---\n");
if ((problem = sparsity("Normal Equations", normal)))
aborterror (problem, "Couldn't display normal equations?");
if ((problem = solve()))
aborterror (problem, "Problem couldn't be solved.");
printf("--- Problem solved -----\n");
/* DEBUG:
dumpeqn ("Normal equations after solution.",
numpoints*3, numpoints*3, (double *)&solution);
END DEBUG */
if ((problem = output()))
aborterror (problem, "Couldn't output result?");
return NoError;
}
/* ========================================================================= */
/* ============================ Main Program =============================== */
/* ========================================================================= */
error main () {
error problem;
printf ("--- Welcome to Demo program two, Version of April 22nd, 2000 -- ");
printf ("This program can be found at:");
printf ("http://www.cc.utah.edu/~nahaj/cave/survey/code/demo2.html\n");
printf ("\nThis program does a Weighted Linear Least Squares Network\n");
printf ("adjustment by observations, given the shot delta's and\n");
printf ("the assumed variances.\n\n");
printf ("This is a toy program, only handling surveys with up to\n");
printf ("%d shots, and %d points.\n\n", MAXSHOTS, MAXPOINTS);
if ((problem = userhelp(1)))
aborterror (problem, "Couldn't output user prompt?");
tryonemore = 1;
while (tryonemore) {
if ((problem = processasurvey()) && problem != ERRendoffile
&& problem != ERRnotfound) {
printf ("**** Survey processing bailed out.\n");
printerror (problem);
}
}
printf ("Done.\n");
exit (NoError);
}
This page is http://www.cc.utah.edu/~nahaj/cave/survey/code/c/demo2.c.html
© Copyright 2000 by John Halleck, All Rights Reserved.
This snapshot was last modified on August 23rd, 2000
And the underlying file was last modified on May 11th, 2000