demo2.c:


/* 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);
}

Go to ...


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