/* demo1.c - Allow people to play with adjustment.
* © Copyright 1999 by John Halleck - All rights reserved.
* http://www.cc.utah.edu/~nahaj/cave/survey/code/c/demo1.c.html
* This code is in support of the introductory survey text at
* http://www.cc.utah.edu/~nahaj/cave/survey/intro/
*/
/* Version of September 9th, 1999 */
/* This program allows one to play with network adjustment and weights.
* It does a full three dimensional weighted least squares solution
* of a LINEARIZED network.
*
* It is a toy program, not intended for serious work.
* (It has very small matrices, and is not done in a manner that
* would be efficient for large data.)
* It is merely an example of the techniques.
*
* The program takes in a survey line of the forms:
* frompointnumber topointnumber x y z wx wy wz wxy wxz wyz
* 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 weight
* 1 2 0 0 20 5 # We go straight up, with a weight 5.
* 2 3 0 10 0 2 3 4 # North with different weights for each coordinate.
* 3 4 10 0 0 10 9 8 1 3 2 # East with full 3d weight 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.
*/
#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"
/* 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 "qinv.h"
/* We will need a matrix inverse */
#include "partition.h"
/* And we make use of matrix partitioning. */
/* - */
/* Are we doing multiple surveys? */
int tryonemore = 0;
static double weightedsumofsquares; /* What we are, after all, minimizing */
static double populationvariance;
#define MAXPOINTS 21
/* 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 */
#define MAXSHOTS 100
/* Maximum number of shots we can handle. */
static int numshots; /* Actual number we have. */
static coordinate shots [MAXSHOTS]; /* shots */
static coordinate weightedshots [MAXSHOTS];
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 havenoweights = 0; /* Gave "assuming identity" message */
static int haveoneweights = 0; /* Gave "assuming weight by identity" */
static int havethreeweights = 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 (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. */
havenoweights = 0; /* Haven't given "assuming identity" message */
haveoneweights = 0; /* Haven't given "assuming weight by identity" message */
havethreeweights = 0; /* Haven't given "assuming diagonal" message */
weightedsumofsquares = 0.0; /* Not known yet. */
populationvariance = 0.0; /* Not known either. */
return NoError;
}
/* ========================== User help ==================================== */
static error userhelp (int level) {
printf ("Type in survey in the form:\n");
printf ("From# To# x y z wxx wyy wzz wxy wxz wyz\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 point and the last. The weights are the elements from\n");
printf (" The weight 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 wxx wyy wzz wxy wxz wyz\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) return ERRnil;
if (!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) return ERRnil;
if (!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) weights ------- */
/* 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 grabweights(double *wxx, double *wxy, double *wxz,
double *wyy, double *wyz,
double *wzz
) {
error problem;
if (!wxx || !wxy || !wxz || !wyy || !wyz || !wzz) return ERRnil;
/* Set up good defaults. */
*wxx = 1.0; *wxy = 0.0; *wxz = 0.0;
*wyy = 1.0; *wyz = 0.0;
*wzz = 1.0;
if (thischaracter == EOF) return ERRendoffile;
if ((problem = skipnoneolwhite()) && problem != ERRnotfound)
return problem;
/* Do we have anything? */
problem = fltfrominput(wxx, "Weight");
if (problem) {
*wxx = 1.0;
if (problem != ERRnotfound) return problem;
if ((problem = skiptoeol())) return problem;
if (!havenoweights) {
havenoweights = 1;
printf ("? Lines without a weight assume:\n");
printf ("? [ 1 0 0 ]\n? [ 0 1 0 ]\n? [ 0 0 1 ]\n");
}
return bailoutifjunk(NoError);
}
if ((problem = fltfrominput(wyy, "Y weight"))) {
*wyy = *wxx; *wzz = *wxx;
if (problem != ERRnotfound) return problem;
if ((problem = skiptoeol())) return problem;
if (!haveoneweights) {
haveoneweights = 1;
printf ("? Lines with a single weight 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", *wxx, 0.0, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, *wxx, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, 0.0, *wxx);
}
return bailoutifjunk(NoError);
}
if ((problem = fltfrominput(wzz, "Z weight"))) {
*wzz = 1.0;
if (problem == ERRnotfound) return bailoutifjunk(ERRbaddata);
return problem;
}
/* Ok, Now let's try for the covariances */
if ((problem = fltfrominput(wxy, "weight of relation between x and y"))) {
*wxy = 0.0;
if (problem != ERRnotfound) return problem;
if ((problem = skiptoeol())) return problem;
if (!havethreeweights) {
havethreeweights = 1;
printf ("? Lines without relation weights will be assumed to be\n");
printf ("? diagonal matrices. In this case:\n");
printf ("? [ %f %f %f ]\n", *wxx, 0.0, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, *wyy, 0.0);
printf ("? [ %f %f %f ]\n", 0.0, 0.0, *wzz);
}
return bailoutifjunk(NoError);
}
/* Ok, we have one, let's get the rest. */
if ((problem = fltfrominput(wxz, "Weight of relation between x and z"))) {
*wxz = 0.0;
if (problem == ERRnotfound) return bailoutifjunk(ERRbaddata);
return problem;
}
if ((problem = fltfrominput(wyz, "Weight of relation between y and z"))) {
*wyz = 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 wxx, wyy, wzz, wxy, wyz, wxz; /* 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 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);
}
/* Get what the user is willing to give for a weight */
if ((problem = grabweights (&wxx, &wxy, &wxz, &wyy, &wyz, &wzz))) {
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;
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);
printf ("Weight: [ %f %f %f ]\n", wxx, wxy, wxz);
printf (" [ %f %f %f ]\n", wxy, wyy, wyz);
printf (" [ %f %f %f ]\n", wxz, wyz, wzz);
if (wxx <= 0.0)
{ printf ("? X weight must be positive\n"); return bailout(NoError); }
if (wyy <= 0.0)
{ printf ("? Y weight must be positive\n"); return bailout(NoError); }
if (wzz <= 0.0)
{ printf ("? Z weight must be positive\n"); return bailout(NoError); }
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);
}
if (numshots > MAXSHOTS) {
printf ("*** Too many shots for this toy program to deal with.\n");
printf ("*** Max is %d\n", MAXSHOTS);
return bailout(ERRoverflow);
}
if ((problem = loadwht (weights[numshots], wxx, wxy, wxz, wyy, wyz, wzz)))
return problem;
shots[numshots][0] = x; shots[numshots][1] = y; shots[numshots][2] = z;
frompoint[numshots] = from; topoint[numshots] = to;
numshots++;
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", weights[i][0], weights[i][3], weights[i][5],
weights[i][1], weights[i][2], weights[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 ("N");
if (j%3==2) printf (" ");
thisone++;
}
printf ("]\n");
if (i%3==2) 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);
if (numshots == numpoints) {
printf ("This survey had no redundancy... Therefore no detectable ");
printf ("error.\n");
} else {
printf ("Estimated weighted population variance = %f\n",
populationvariance);
printf ("Estimated weighted standard error: %f\n",
sqrt(populationvariance));
printf ("Degrees of freedom of this survey = %d\n", numshots-numpoints);
}
printf ("\n");
if ((problem = outpoints(numshots,
"Final solution in terms of shots",
(double *) &newshots))
) return problem;
if ((problem = outpoints(numpoints,
"Final solution in terms of points",
(double *) &locations))
) 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;
}
/* Population variance is handy */
/* We probably ought to compute the aposteriori reference variance also. */
populationvariance = weightedsumofsquares / (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");
if ((problem = output()))
aborterror (problem, "Couldn't output result?");
return NoError;
}
/* ========================================================================= */
/* ============================ Main Program =============================== */
/* ========================================================================= */
error main () {
error problem;
printf ("--- Welcome to Demo program one, Version of September 9th, 1999: ");
printf ("This program does a Weighted Linear Least Squares Network\n");
printf ("adjustment by observations, given shot deltas and weights\n");
printf ("Adjustments given shot deltas and covariances are done by\n");
printf ("the demo2 program. This demo is left around for people really\n");
printf ("wanting to get a feel for weighting issues.\n\n");
printf ("This program can be found at:\n");
printf ("http://www.cc.utah.edu/~nahaj/cave/survey/code/demo1.html\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);
}
/* ********************* END ************************************************ */
This page is http://www.cc.utah.edu/~nahaj/cave/survey/code/c/demo1.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