/* flatfish.c  last updated 6/20/1997 hpm - fixed rotation bug */

#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <pbm.h>
#include <pgm.h>
#include <ppm.h>
#include <assert.h>

#define SPOTRADIUSMIN  4	/* minimum radius of spot to consider */
#define DISTORPOLYDEG  5	/* degree of radial distortion polynomial + 1 */

#define PhMax	1000  /* Maximum picture height */
#define PwMax	2000  /* Maximum picture width */
#define Pad 40  /* Picture padding border, pixels */
#define Sad 2   /* How far beyond boundaries to allow spot centers */
#define Paspect 1.020 /* Width/height ratio of pixels */

gray **picin;  	/* raw incoming picture */
pixel **picout;	/* outgoing display picture */
gray **graphout;	/* outgoing plot picture */
int	Ph,	/* Picture height, pixels (576 or 480 typical) */
        Pw;	/* Picture width, pixels  (768 or 640 typical) */

unsigned char *Bch[PhMax+2*Pad];  /* row pointers to padded image */
unsigned char **picpad;    /* padded image, for working beyond edges */     


#define PI 3.14159265358979  /* Pies are round */

/* Radial distortion finder and rectifier for solid state cameras
   with wide angle lenses. Created 5/95
                              Hans Moravec <hpm@cmu.edu>
                              Carnegie Mellon University
			      Pittsburgh, PA  15213

   In calibration phase, the program assumes the camera
   is perpendicularly aimed at a square, horizontally oriented,
   grid of round dark spots, with an additional spot at a half
   grid position near the center:

   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * *_* * * * * * * * * * * *
   * * * * * * * * * * * *^* * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *
   * * * * * * * * * * * * * * * * * * * * * * * *

   (the center "spot" made of an underbar paired with a hat is
   supposed to look like all the others, represented by stars,
   and all spots should be black circular disks: excuse the
   limitations of ascii graphics).  The program was developed
   with a test pattern made of a magnetic white board 4 feet high
   and six feet wide, covered with a 16 x 24 + 1 grid of dark
   magnetic disks 1.5 inches in diameter at 3 inch spacing.
   The object of the calibration was a stereo bar of three Sony
   XC-75 black and white cameras with 90 degree wide angle lenses
   exhibiting strong fish eye distortion, with a radial image
   compression factor between the center and the edges of at least
   2:1. The cameras were mounted parallel at 15 cm spacing, and
   connected to the R, G and B channels of a color digitizer. The
   camera bar was placed so the lens iris rings were 22.5 inches
   in front of the calibration pattern.  This filled each camera's
   field of view with a majority of the pattern.   The center spot
   was approximately centered in the "green" camera's image, to
   the left of center in the "blue" image and to the right in the
   "red" image.
 
   The program finds the spots, in pixel (distorted) and spot grid
   (ideal) coordinates, and finds a least-squares best fit distortion
   center, assumed to indicate lens axis, a digitizer aspect ratio,
   and a distortion correcting polynomial that maps distorted to ideal
   radius from that center.  In our setup the result RMS deviation for
   all (approximately 300/image) spots is better than 0.5 pixel.

   In our setup, we note that the lens center between calibrations
   jitters only a few hundredths of a pixel.

   As of 11/95 the program could rectify images from the cameras using
   the distortion polynomial into ones with ideal projective geometry.

   The program is not complete, as of 3/96.  Bugs and missing:

   When the distorion causes a corner spot to appear in the calibration
   image whose horizontal and vertical neighbors are off the edge of the
   image, the program often links it with a diagonal neighbor, giving it
   the wron ideal coordinates.  Tests need to be added to handle this
   situation. (fixed 3/15/96 - hpm).

   The center spot is intended to relate the three cameras, in a not
   yet implemented later calibration step, which produces a full
   geometric camera calibration.  (completed 4/24/96 - hpm)

*/


/* The Great Aspect Ratio Saga

   Concerning aspect ratio of Sony XC-75 camera module in conjunction
   with our K2T digitizer: the camera specs say the camera pixels are
   8.4 um horizontal by 9.8 um vertical.  Horizontal clock rate is
   14.318 MHz.  Vertical drive frequency is 15.734 kHz +- 1%.
   The camera has 768 horizontal by 494 vertical pixels.
   Sensing area is 1/2 inch wide.  (Optical blank 43 elements
   on each horizontal line?)  EIA system.  Chip size is 7.95 (h) by
   6.45 (v) mm.  525 lines.
---
    Turns out that the resolution of AQDOTFREQ in the k2t package
    is limited  - I think it's 80ns and the 12.2 MHz is really 12.256
    which is as close as we can get to perfectly square...   Bill Ross

12.256 makes the horizontal pixel size 9.813, vs.  9.8 um vertical, an
error of only 0.135 percent, about 0.86 pixels total in the horizontal
direction, if you use the vertical as the reference.  That's
good, it means that there is no effect on things like template
matches, where the template spans only a fraction of the image, and
thus has a maximum aspect stretch much less than a pixel.  In numeric
computations involving pixel coordinates, I can now multiply in the
aspect ratio of 1.001352107, and have complete peace of mind.
A camera solver would never have been able to come up with an
estimate that accurate.  

hpm 9/95 Hah!!!! Minimizing over aspect ratio with the distortion-finding
 code gives a repeatable aspect ratio of 1.020 +/- .0006.  Using that
 found aspect ratio reduces the rms error of the best fit curve of spot
 index radius to pixel radius to about 0.5 pixel, compared to much worse
 2.5 pixels using the calculated aspect ratio.  Another good intention
 leads to hell!

*/


float **flP;  /* raster spotness value */
unsigned char **chP;  /* raster spotness radius */

int Nspots = 0;		/* number of spots found */
double *Ispots, *Jspots;   /* spot location list */
double *Vspots, *Rspots;  /* spotness value, radius lists */
double *IIspots, *IJspots, *JIspots, *JJspots; /* spot local coordinate system */

int *Aspots;  /* spot coordinate status: -1 untouched, 0 inited, 1 done */

float **Dspots;  int **DPspots, **DPerror;
int Cspots, Pspots;   /*  Pattern and image center spots */


double Poly[1000];  /* Snapshot of "fitpoly" polynomial */
float isc, jsc;  int deg;  /* Image axis center, and radial polynomial degree */
double ipc, jpc, res, aspect, rootaspect; 
double bestisc, bestjsc, bestaspect, bestfit;
int dpass;


double CalibPi, CalibPj, CalibAS;
/* Calibrated pixel image center and aspect ratio */
double CalibSi, CalibSj;   /* Calibrated spot image center */
int CalibPN = 0;  /* Scatter polynomial size */
double CalibP[100];  /* Spot->Pixel radius scatter plot polynomial */
double CalibS[100];  /* Pixel->Spot radius scatter plot polynomial */
double CalibAN;	     /* Angle of the gid image from horizontal (vertical) */

#define x 255
#define y 128
unsigned char cmap[15][3] =
{{0,0,0},{x,0,0},{0,x,0},{0,0,x},{0,x,x},
 {x,0,x},{x,x,0},{x,x,x},{x,y,0},{x,0,y},
 {y,x,0},{y,0,x},{0,x,y},{0,y,x},{y,y,y}};
#undef x
#undef y

/* spot pixels, from center out, maximally balanced */
#define SPX 5000	/* maximum length of spot list */
char spi[SPX], spj[SPX];	/* i and j coords of the pixels */
int sprpos[100];	/* starting index for each whole pixel ring */
int spn = 0;		/* actual number of pixels in list */
int sprmax = 0;		/* maximum spot radius in list */

int i, j, k, l, m, n;	

FILE *fp, *flog;
char slog[500];  /* flog, slog: file and string for run log output */

void plog()      /* write out message in log file and stdout */
{   fprintf(flog,"%s",slog); printf("%s",slog);   };

void llog(char *s) /* write fixed string in log file */
{   fprintf(flog,s); printf(s);   };

void **padmat(int row, int col, int padrow, int padcol, int typesize)
  /* make padded 2D array of typesize elements */
{	
    int i, sizfac;
    void *ar, **arp;
    arp = (void **) calloc(row+2*padrow,sizeof(void *));
    ar =  (void *) calloc((row+2*padrow)*(col+2*padcol),typesize);
    sizfac = typesize/sizeof(void);
    for (i=0; i<row+2*padrow; i++)
      arp[i] = &(ar[(i*(col+2*padcol)+padcol)*sizfac]);
    return( &(arp[padrow]));
};

void rings(int ro)  /* make bullseye for spots, radius ro pixels */
{
    int i, j, jbst,sumi,sumj,t;  float is,js,ros,ti,tj,dc,err;

    sprmax = sqrt(SPX/3.2) - 1;
    if (sprmax < ro)  ro = sprmax; else sprmax = ro;
    ros = (ro+1)*(ro+1);  spn = 0;     
    for (i=0; i<ro+1; i++) sprpos[i]=0;

    for (i = -ro-1; i <= ro+1; i++)  /* mark out cells in circle */
      {   is = i*i;
	  for (j = -ro-1; j <= ro+1; j++)
	    {   js = j*j;
	 	if (is+js <= ros) { spi[spn] = i;  spj[spn] = j;  spn++; }
	    }
      }

    sumi = sumj = 0;
    for (i=0; i<spn-1; i++)  /* accrete to keep centroid at origin */
      {   err = 1<<30; jbst = i;
	  for (j=i+1; j<spn; j++)
	    { 
		ti = sumi+spi[j];   tj = sumj+spj[j];
		dc = ti*ti + tj*tj;
		if (dc<err) { err=dc;  jbst=j; };
	    }
	  t = spi[i];  spi[i] = spi[jbst];  spi[jbst] = t;
	  t = spj[i];  spj[i] = spj[jbst];  spj[jbst] = t;
	  sumi += spi[i];   sumj += spj[i];
	  ti = spi[i];  tj = spj[i];  t = sqrt(ti*ti+tj*tj);
	  if (sprpos[t]==0) sprpos[t]=i;
      };
}


double fitspot(int pi, int pj, int lor, int hir, int *rbest)
  /* match spot prototype to image location pi,pj */
  /* consider spot radii from lor to hir */
{
    int i, k, outr, ant, avt, it;
    double sn[100], sv[100], sq[100];
    double t, an, av, aq, dn, dv, dq, on, ov, oq;
    double dotq, dotbest, dmn, dvr, omn, ovr;

    outr = 1.4*hir;  if (outr>sprmax) outr = sprmax;
    aq = avt = ant = 0;

    for (i = 0; i<outr; i++)
      {
	  for (k=sprpos[i]; k<sprpos[i+1]; k++)
	    if ((it=picpad[pi+spi[k]][pj+spj[k]]) != 128)
	      {  t=it;  aq += t*t;   avt += it;    ant++; }
	  sq[i] = aq;	  sv[i] = avt;	  sn[i] = ant;
      };
    an = ant;  av = avt;
    /* compute count, sum and sum of squares of pixel values
       in concentric rings around candidate spot (dot) position */

    dotbest = -1000000.0;  *rbest = 0;

    /* scan range of candidate radii */
    for (k = lor; k<=hir; k++)
      {  dn = sn[k];  dv = sv[k];  dq = sq[k];
	 on = an-dn;  ov = av-dv;  oq = aq-dq; 

	 dmn = dv/dn;	  dvr = (dq - dv*dmn);
	 omn = ov/on;	  ovr = (oq - ov*omn);
	  
	 dotq = omn-dmn - sqrt(9.0*dvr/(dn-1.0)+3.0*ovr/(on-1.0));
	 if (dotq>dotbest) { dotbest = dotq;  *rbest = k;};
      }

    return(dotbest*(*rbest));  /* bias answer towards largest spots */
}
    

void paintspots(int lor, int hir)  /* calculate "spotness" across image */
{ int i,j,l;

    flP = (float **) padmat(Ph,Pw,Sad,Sad,sizeof(float));
    chP = (unsigned char **) padmat(Ph,Pw,Sad,Sad,sizeof(char));

    for (i= -Sad; i<Ph+Sad; i++) for (j= -Sad; j<Pw+Sad; j++)
      {
	  flP[i][j] = fitspot(i,j,lor,hir,&l);  chP[i][j] = l;
      }	
}

void spotmaxlink()  /* link maxima of paintspots operator into grid */
{
    double v, spsep;  
    int i, ii, j ,jp, jj, k, l, ndone;  double di, dj, ddi, ddj, d;
    float *Dsp;    int *DPsp;

    int Hood = 8;     /* number of nearest spot neighbors to consider
			 when searching for adjacent neighbors */

    /* create arrays for spot parameter lists */
    Ispots = (double *) calloc(Nspots,sizeof(double));
    Jspots = (double *) calloc(Nspots,sizeof(double));
    Vspots = (double *) calloc(Nspots,sizeof(double));
    Rspots = (double *) calloc(Nspots,sizeof(double));

    /* create spot local coordinate system lists */
    IIspots = (double *) calloc(Nspots,sizeof(double));
    IJspots = (double *) calloc(Nspots,sizeof(double));
    JIspots = (double *) calloc(Nspots,sizeof(double));
    JJspots = (double *) calloc(Nspots,sizeof(double));

    Aspots = (int *) calloc(Nspots,sizeof(float));

    Nspots = 0;
    for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) if (flP[i][j]>0.0)
      { Ispots[Nspots] = i;  Jspots[Nspots] = j;
	Vspots[Nspots] = flP[i][j];
	Rspots[Nspots] = chP[i][j];   Nspots++; };

    /* print histogram of found spot sizes */
    {
    int sizes[100], szmax, szmin;
    szmin = 99;  szmax = 0; for (i=0; i<100; i++) sizes[i]=0;
    for (i=0; i<Nspots; i++)
      {	sizes[(int) Rspots[i]]++;
	if (Rspots[i]<szmin) szmin=Rspots[i];
	if (Rspots[i]>szmax) szmax=Rspots[i]; };
    llog("Spot histogram (of maxima): ");
    for (i=szmin; i<=szmax; i++) { sprintf(slog," %d(%d)",i,sizes[i]); plog(); };
    llog("\n");
    }

    /* create arrays for spot neighborhoods */
    Dspots = (float **) padmat(Nspots,Hood,0,0,sizeof(float));
    DPspots = (int **) padmat(Nspots,Hood,0,0,sizeof(int));
    DPerror = (int **) padmat(Nspots,5,0,0,sizeof(int));

    /* and temporary arrays for spot distance sort */
    Dsp = (float *) calloc(Nspots,sizeof(float));
    DPsp = (int *) calloc(Nspots,sizeof(int));

    llog("\n* Linking neighboring spots into grid *\n");

    llog("Creating and propagating spot-local coordinate systems\n");

    /* scan spot list to find neighbors of each spot */
    if (Nspots < Hood) Hood = Nspots;

    for (i = 0; i<Nspots; i++)  /* for each spot */
      {
	  for (j=0; j<Nspots; j++)
	    { /* calculate spot-size normalized distance to other spots */
		DPsp[j] = j;
		di = Ispots[j]-Ispots[i];  dj = Jspots[j]-Jspots[i];
		Dsp[j]  = sqrt(di*di + dj*dj) / (Rspots[i]+Rspots[j]+1);
		/* spot size sets scale */
	    };

	  /* find and save Hood closest neighbors (sort them, sort of) */
	  for (j=0; j<Hood; j++) for (k=j+1; k<Nspots; k++)
	    if (Dsp[k] < Dsp[j])
	      {
		  v = Dsp[k];  Dsp[k] = Dsp[j];   Dsp[j] = v;
		  l = DPsp[k]; DPsp[k] = DPsp[j]; DPsp[j] = l;
	      };

	  for (j=0; j<Hood; j++)
	    {  Dspots[i][j] = Dsp[j];  DPspots[i][j] = DPsp[j]; };

      }

    free(Dsp);  free(DPsp);

    llog("Pick out and isolate pattern center spot\n");

    Cspots = 0;  /* find cosiest spot -- better be pattern center */
    for (i=1; i<Nspots; i++)
	  if (Dspots[i][4]<Dspots[Cspots][4]) Cspots = i;

    sprintf(slog,
	    "Pattern Center spot %d (spacing %.3g spot diameters, %.3g pixels)\n",
	    Cspots,Dspots[Cspots][4],
	    Dspots[Cspots][4]*(Rspots[Cspots]+Rspots[DPspots[Cspots][4]]+1));
    plog();

    /* remove center spot from neighbor's lists */

    for (i = 0; i < Nspots; i++) if (i != Cspots)
      for (j=1; j<Hood; j++) if (DPspots[i][j] == Cspots)
	for (k=j+1; k<Hood; k++)
	  { Dspots[i][k-1] = Dspots[i][k];
	    DPspots[i][k-1] = DPspots[i][k]; }

    spsep = l = 0;
    for (i=0; i<Nspots; i++) if (i != Cspots) /* trim and average spot spacing */
      {  spsep += Dspots[i][1];  l++;  }
    /* accumulate mean spots spacing */
    spsep /= l;  /* find spot spacing mean */
    sprintf(slog,"Mean spacing %.3g spot diameters\n",spsep); plog();

    Pspots = 0;   ddi = 1<<30;   /* find image centermost spot */
    for (i=0; i<Nspots; i++) if (i != Cspots)
      {
	Aspots[i] = -1;  /* indicate every spot has no coordinate frame yet */
	di = Ispots[i] - Ph/2;   dj = Jspots[i] - Pw/2;
	d = di*di + dj*dj;
	if (d<ddi) {ddi = d;  Pspots = i;};
      };
    sprintf(slog,"Centroid spot %d, at pixel center + (%g,%g)\n",Pspots,
	   Ispots[Pspots]-Ph/2,Jspots[Pspots]-Pw/2);  plog();

    /* set up initial local coord axes for first spot */
    IIspots[Pspots] = JJspots[Pspots] = spsep*2*Rspots[Pspots];
    IJspots[Pspots] = JIspots[Pspots] = 0.0;  /* straight up! */
    Aspots[Pspots] = 0;  /* first spot now has initial coordinates */

    llog("Spot linking iterations:");
    do
      {   double axpr[4][2], axbs[4][2], axvl[4];   int axid[4];
	  /* local coord axes,  prototype, and best found, value, and id */

	  ndone = 0;
	  for (i=0; i<Nspots; i++) if (i != Cspots)
	    if (Aspots[i] == 0)  /* refine initial coords for spot i */
	      {  ndone++;
		 axpr[0][0]= IIspots[i]; axpr[0][1]=IJspots[i]; /* +I (y) axis */
		 axpr[1][0]= -IIspots[i]; axpr[1][1]= -IJspots[i]; /* -I (-y) */
		 axpr[2][0]= JIspots[i]; axpr[2][1]= JJspots[i]; /* +J (x) */
		 axpr[3][0]= -JIspots[i]; axpr[3][1]= -JJspots[i]; /* -J (-x) */

		 /* test and note if this spot's spacing aspect ratio is weird */
		 ddi = sqrt(axpr[0][0]*axpr[0][0] + axpr[0][1]*axpr[0][1]);
		 ddj = sqrt(axpr[3][0]*axpr[3][0] + axpr[3][1]*axpr[3][1]);
		 if (ddi/ddj > 1.4 || ddj/ddi > 1.4)
		   {sprintf(slog,"Point %d, axis ratio %.3g\n",i,ddi/ddj); plog();};

		 for (j=0; j<4; j++) /* initialize best axis spots */
		   {   /* fits must be better than 3/4 way to prototype */
		       di = axbs[j][0] = axpr[j][0]*0.75;
		       dj = axbs[j][1] = axpr[j][1]*0.75;   axid[j] = -1;
		       ddi = axpr[j][0] - di;     ddj = axpr[j][1] - dj;
		       axvl[j] = ddi*ddi + ddj*ddj;
		   };

		 /* search for best fit for each axis direction among neighbors */
		  for (j=1; j<Hood-1; j++)
		   {
		       l = DPspots[i][j];
		       di = Ispots[l]-Ispots[i];  dj = Jspots[l]-Jspots[i];
		       for (k=0; k<4; k++)
			 {
			     ddi = axpr[k][0] - di;   ddj = axpr[k][1] - dj;
			     d = ddi*ddi + ddj*ddj;
			     if (d<axvl[k])
			       { 
				   axvl[k] = d;  axid[k] = l;
				   axbs[k][0] = di;  axbs[k][1] = dj;
			       };
			 };
		   };

		 for (j=0; j<4; j++) /* mix best coord fits into prototype */
		   if (axid[j]>=0) for (k=0; k<2; k++) axpr[j][k] = axbs[j][k];

		 for (j=0; j<4; j++)  DPspots[i][j+1] = axid[j];
		 Aspots[i] = 1;  /* this spot has finalized coordinates! */

		 IIspots[i] = (axpr[0][0]-axpr[1][0])/4.0 + IIspots[i]/2.0; 
		 IJspots[i] = (axpr[0][1]-axpr[1][1])/4.0 + IJspots[i]/2.0;
		 JIspots[i] = (axbs[2][0]-axpr[3][0])/4.0 + JIspots[i]/2.0;
		 JJspots[i] = (axbs[2][1]-axpr[3][1])/4.0 + JJspots[i]/2.0;

		 for (j=0; j<4; j++)  /* propagate to neighbors */
		   if ((k=axid[j])>=0) {if (Aspots[k]<0) /* uninited */
		   {   Aspots[k] = 0;
		       IIspots[k] = IIspots[i];  IJspots[k] = IJspots[i];
		       JIspots[k] = JIspots[i];  JJspots[k] = JJspots[i];
		   }
		 else if (Aspots[k]==0) /* already has initial coordinates */
		   { /* average in this spot's coordinates */
		       IIspots[k] = (IIspots[i] + IIspots[k])/2;
		       IJspots[k] = (IJspots[i] + IJspots[k])/2;
		       JIspots[k] = (JIspots[i] + JIspots[k])/2;
		       JJspots[k] = (JJspots[i] + JJspots[i])/2;
		   }; };

		 for (j=0; j<4; j++) DPspots[i][j+1] = axid[j];
		 /* replace near neighbor with coordinate neighbor links */
	      };
	  sprintf (slog," %d",ndone); plog();
      }  while (ndone>0);
    llog("\n");

    /* edit out link inconsistencies */
    do
      {	  
	  for (i=0; i<Nspots; i++) for (j=0; j<=4; j++) DPerror[i][j] = 0;

	  /* count link inconsistencies */
	  for (i=0; i<Nspots; i++) if (i!=Cspots)
	    for (j=1; j<=4; j++)
	      if ((k=DPspots[i][j]) >= 0)
		if ((l=DPspots[k][jp=1+((j-1) ^ 1)]) >= 0) if (l != i)
		  {  DPerror[k][jp]++; DPerror[i][j]++; };

	  ii = jj = 0;  /* find the most inconsistent link */
	  for (i=0; i<Nspots; i++) for (j=1; j<=4; j++)
	    if (DPerror[i][j] > DPerror[ii][jj])  { ii = i;  jj = j; };

	  DPspots[ii][jj] = -1;  /* wipe out max inconsistency */
	  if (jj>0) { sprintf(slog,"Link inconsistency %d[%d] erased\n",ii,jj);
		      plog(); };

      }  while (jj>0);


   /* complete unidirectional links, flag overlooked link inconsistencies */
    for (i=0; i<Nspots; i++) if (i!=Cspots)
      for (j=1; j<=4; j++)
	if ((k=DPspots[i][j])>=0)
	  {
	      if ((l=DPspots[k][1+((j-1) ^ 1)]) == -1)
		{ DPspots[k][1+((j-1) ^ 1)] = i;
		  sprintf(slog,"Missing link %d --> %d\n",k,i); plog(); }
	      else if (l != i)
		  { sprintf(slog,
		     "GRID LINKING INCONSISTENCY %d -> %d <- %d !!!!\n",i,k,l);
		    plog(); }
	  }

    llog("Assigning ideal coordinates to spots\n");

     /* label spots with ideal coordinates */
    for (i=0; i<Nspots; i++) Aspots[i] = 0;

    IIspots[Pspots] = JJspots[Pspots] = 0;
    Aspots[Pspots] = 2;  /* image center spot is now defined as ideal [0,0] */

    llog("Ideal labelling iterations:");
    do
      {   /* label adjoining spots with ideal coordinates */ 
	  ndone = 0;
	  for (i=0; i<Nspots; i++) if (i != Cspots)
	    if (Aspots[i] == 2)
	      for (j=1; j<=4; j++) if ((k=DPspots[i][j])>=0)
		{   
		    di = IIspots[i]; dj = JJspots[i];
		    l = ((j-1)&1)?-1:1;
		    if ((j-1)&2) dj += l; else di += l;
		    if (Aspots[k]==2)
		      { if (IIspots[k] != di || JJspots[k] != dj)
			  {  sprintf(slog,
				     "GRID LABELING CLASH [%g,%g] : [%g,%g] !!!!\n",
				     di,dj,IIspots[k],JJspots[k]);
			     plog(); }
		      }
		    else
		      { Aspots[k]=2; ndone++;
			IIspots[k] = di;  JJspots[k] = dj; };
		};
	  sprintf(slog," %d",ndone); plog();
      } while (ndone>0);
    llog("\n");

    /* calculate ideal coords for pattern center spot */
    IIspots[Cspots]=JJspots[Cspots]=0;
    for (j=1; j<=4; j++)
      {
	  IIspots[Cspots] += IIspots[DPspots[Cspots][j]];
	  JJspots[Cspots] += JJspots[DPspots[Cspots][j]];
      };
    IIspots[Cspots] /= 4.0;       JJspots[Cspots] /= 4.0;
    sprintf(slog,"Pattern center spot, at Centroid spot + [%.3g,%.3g]\n",
	   IIspots[Cspots],JJspots[Cspots]); plog();

    /* shift spot coordinates so pattern center spot is at (0,0) */
    di = IIspots[Cspots];  dj = JJspots[Cspots];
    for (i=0; i<Nspots; i++) { IIspots[i] -= di;  JJspots[i] -= dj; };
}


void spotpixelcoord(double *ipos, double *jpos)
/* convert spot coords to pixel coords, interpolating between spots */
{
    int i;
    double is, js;
    double is0, is1, js0, js1;   int p00, p01, p10, p11;

    is = *ipos;  js = *jpos;   /* set up spot coordinates */

    /* set up bounding spot coordinates */
    is0 = floor(is-0.5) + .5;  is1 = is0+1.0;
    js0 = floor(js-0.5) + .5;  js1 = js0+1.0;

    /* find array index of bounding spots */
    p00 = p01 = p10 = p11 = -1;
    for (i =0; i<Nspots; i++) if (i != Cspots)
      {
	  if (IIspots[i]==is0 && JJspots[i]==js0) p00 = i;
	  if (IIspots[i]==is0 && JJspots[i]==js1) p01 = i;
	  if (IIspots[i]==is1 && JJspots[i]==js0) p10 = i;
	  if (IIspots[i]==is1 && JJspots[i]==js1) p11 = i;
      };
    
    if (p00<0 || p01<0 || p10<0 || p11<0)
      { llog("Spotpixel not bounded! ");
	if ((i = p00>p01?p00:p01) < 0) i = p10>p11?p10:p11;
	if (i<0) return;
	*ipos = Ispots[i];  *jpos  = Jspots[i];
	return; };

    is -= is0;  js -= js0;  /* fractional position within bounding box */

    /* Bilinear interpolation of spot pixel coordinates */

    *ipos  = (Ispots[p00]*(1.0-is) + Ispots[p10]*is)*(1.0-js) +
      (Ispots[p01]*(1.0-is) + Ispots[p11]*is)*js ;

    *jpos  = (Jspots[p00]*(1.0-is) + Jspots[p10]*is)*(1.0-js) +
      (Jspots[p01]*(1.0-is) + Jspots[p11]*is)*js ;

};


int SimulSolve(double a[], int n)  /* solve n simultaneous linear equations */
/* a is an [n * (n+1)] array with rhs constants in rightmost column.
   Solved values are returned in a[0:n-1]. Rest of a turns into a mess. */
    { int i, ii, j, jj, k, it;  double t, tt;    int perm[n];

      /* initialize pivot permutation */
      perm[0]=0;  for (i=1; i<n; i++) perm[i] = perm[i-1]+n+1;
      
      for (ii=0; ii<n; ii++)  /* elimination loop */
        { if ((tt = a[perm[ii]+ii])<0.0) tt = -tt;
          for (i=ii+1; i<n; i++)  /* pivot */
            { if ((t = a[perm[i]+ii])<0.0) t = -t;
                if (t>tt)
               {it = perm[ii]; perm[ii] = perm[i]; perm[i] = it; tt=t;}; }
          i = perm[ii]; if ((t=a[i+ii]) == 0.0) return(0); /* singular */
          if (t != 1.0)
            { t = 1.0/t;  /* eliminate variable perm[ii] */
              for (k=ii+1; k<=n; k++) a[i+k] *= t; }
          for (jj=ii+1; jj<n; jj++)
             { j = perm[jj]; if ((t=a[j+ii])!=0.0)
                for (k=ii+1; k<=n; k++) a[j+k] -= t*a[i+k]; }
         }
       for (ii=n-1; ii>0; ii--)   /* backsolve */
         { i = perm[ii]; if ((t=a[i+n]) != 0.0)
              for (jj=0; jj<ii; jj++)  
                {j = perm[jj]; a[j+n] -= a[j+ii]*t;}
         }
       /* unscramble pivot permutation */
       for (ii=0; ii<n; ii++) a[ii]=a[perm[ii]+n];
       return(1);
    };


double fitpoly(int deg, int n, double *r, double *rp, double *poly)
/* fit least-squares degree deg-1 polynomial to n points r -> rp,
   and return its root mean square error */
{	double S[2*deg-1], R[deg], A[deg*(deg+1)];
	int i, j, k;   double v, x, xp, sum;

	for (k=0; k<2*deg-1; k++) S[k]=0;
	for (k=0; k<deg; k++) R[k]=0;

	for (i=0; i<n; i++) /* compute power sums for matrix and rowsums */
	  { v = 1.0;  /* 1.0 becomes w[i] if weighted sums needed */
	    x = r[i];  xp = rp[i];
	    for (k=0; k<2*deg-1; k++)
	      { S[k] += v;  if (k<deg) R[k] += v*xp;  v *= x; }; };

	k = 0;  /* set up equations */
	for (i = 0; i<deg; i++)
	  {  for (j=0;  j<deg;  j++) A[k+j] = S[i+j];
	     k += deg+1;  A[k-1] = R[i];  /* right hand side */ };

	SimulSolve(A,deg);	/* and solve them */

	for (i=0; i<deg; i++) poly[i] = A[i];  /* return the polynomial */

	sum = 0;	/* compute root mean square error */
	for (i=0; i<n; i++)
	  {   x = r[i];  xp = A[deg-1];   /* evaluate polynomial */
	      for (j = deg-2; j>=0; j--)  xp = xp*x + A[j];
	      xp -= rp[i];
	      sum += xp*xp;  /* *w[i] if weighted sum */   };

	return (sqrt(sum/n)); /* /Sum(w[i]), not n, if weighted sum */
};


void showspots(int lor, int hir) /* color spots found by paintspots */
{
    int i, j, k, l, szmax;  double flmax;
    int sizes[100];

    for (i=0; i<100; i++) sizes[i]=0;  flmax = 0.0;  szmax = 0;

    for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) if (flP[i][j]>0)
      {   if (flP[i][j]>flmax) flmax=flP[i][j];
	  if (chP[i][j]>szmax) szmax=chP[i][j];
	  sizes[chP[i][j]]++; 
      }
    if (hir>szmax) hir=szmax;

    llog("Spot operator spot size histogram: ");
    l=0; while (sizes[l]==0 && l<100) l++;
    for (i=l; i<=szmax; i++) 
      { sprintf(slog," %d(%d)",i,sizes[i]); plog(); };  llog("\n");

    for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) if (flP[i][j]>0)
      {   k = chP[i][j];  if (k<lor) k = lor;  if (k>hir) k = hir;
	  l = sqrt(sqrt(flP[i][j]/flmax))*255.0;
	  PPM_ASSIGN(picout[i][j],l,(l*(k-lor))/(hir-lor),l);
      };
}


void stroke(int kl, int is, int js, int id, int jd) 
                          /* draw a line from is,js to id,jd */
{ int i,j,k,l;

  l = sqrt((double) ((is-id)*(is-id)+(js-jd)*(js-jd))) + 1;

  for (k=0; k<=l; k++)
    {
	i = (k*id + (l-k)*is)/l;   j = (k*jd + (l-k)*js)/l;
	if (j >= 0 && j < Pw) if (i >=0 && i<Ph)
	  PPM_ASSIGN(picout[i][j],cmap[kl-1][0],cmap[kl-1][1],cmap[kl-1][2]);
    };
};

void crosshair(int kl, int ip, int jp, int w) 
                          /* draw an X of size w at ip,jp */
{ int i;

  for (i=-w; i<=w; i++) if (jp+i >= 0 && jp+i < Pw)
      { if (ip-i>=0 && ip-i<Ph)
	  PPM_ASSIGN(picout[ip-i][jp+i],cmap[kl-1][0],cmap[kl-1][1],cmap[kl-1][2]);
	if (ip+i>=0 && ip+i<Ph)
	  PPM_ASSIGN(picout[ip+i][jp+i],cmap[kl-1][0],cmap[kl-1][1],cmap[kl-1][2]);
    };
};

void ringdraw(int kl, int ip, int jp, int r) 
                          /* draw a ring overlay around ip,jp  */
{ int i,j,k;

  for (k=sprpos[r]; k<sprpos[r+1]; k++)
    {
	i = ip + spi[k];   j = jp + spj[k];
	if (i>=0 && i<Ph && j >= 0 && j < Pw)
	  { if (cmap[kl-1][0]) PPM_PUTR(picout[i][j],cmap[kl-1][0]);
	    if (cmap[kl-1][1]) PPM_PUTG(picout[i][j],cmap[kl-1][1]);
	    if (cmap[kl-1][2]) PPM_PUTB(picout[i][j],cmap[kl-1][2]); };
    };
};



int main(argc, argv)
int argc;
char *argv[];
{
   int i, j, k, ii, jj;  gray maxval;  int rows, cols;
   int time1, time2 ;  double di, dj, ddi, ddj;
   double v; int rmax;
   FILE *fp;  char img[100];  int imgl;	     /* calibration image name */
   float Yaim = 0.0, Xaim = -2.62, Distance = 8.5, FOV = 90.0;
   /* Daimler Benz old camera calibration setup 
      Yaim = 0.0, Xaim = 0.0, Distance = 40.33, FOV = 30.0; */
   /* CMU red camera
      Yaim = 0.0, Xaim = -2.62, Distance = 8.5, FOV = 90.0; */

   if (argc < 2)
     {
     printf("Usage: flatfish spot.pgm <Yaim> <Xaim> <Distance> <FOV>\n");
     printf("spot.pgm: an aligned image of a calibration spot grid\n");
     printf("Y,Xaim: down,right spot coords of camera aim, center spot = (0,0)\n");
     printf("Distance: spot spacings between lens center and the spot grid\n");
     printf("FOV: Degrees horizontal field of view of rectified image\n");

     exit(0);
     };

   strcpy(img,argv[1]); imgl = strlen(img);
   if (imgl>5) if (img[imgl-4]=='.') imgl -= 4;
   img[imgl] = 0;

   pgm_init(&argc, argv);
   ppm_init(&argc, argv);

   /* Read in raw spot image */

   fp = fopen(argv[1],"r");
   if (fp == NULL) {printf("No %s file!!\n",argv[1]); exit(0); };
   picin = pgm_readpgm(fp, &cols, &rows, &maxval );
   fclose(fp);
   Pw = cols;   Ph = rows;  
   if (maxval > 255)
     { printf("Digitized pic pixels too big: %d instead of %d!!\n",
	      maxval,255);  exit(0); };

   if (argc > 2) sscanf(argv[2],"%g",&Yaim);
   if (argc > 3) sscanf(argv[3],"%g",&Xaim);
   if (argc > 4) sscanf(argv[4],"%g",&Distance);
   if (argc > 5) sscanf(argv[5],"%g",&FOV);

   if (Yaim>10 || Yaim<-10 || Xaim>15 || Xaim<-15 
       || Distance<3 || Distance>50 || FOV < 10 || FOV > 180)
     {
     printf("(Yaim %.3g Xaim %.3g Distance %.3g FOV %.3g) seem unreasonable\n",
	    Yaim, Xaim, Distance, FOV);
     exit(0);
     };

   flog = fopen(strcat(img,".log"),"w");  /* open log file */
   img[imgl] = 0;  /* reset file name to base */
   
   llog("\n");
   j = strlen(argv[1]);  /* pretty print file name */
   for (i=0; i<j+6; i++) slog[i]='*'; slog[j+6]=0;
   llog("    "); llog(slog); llog("\n");
   llog("    "); llog("*  ");  llog(argv[1]);  llog("  *\n");
   llog("    "); llog(slog); llog("\n");

   sprintf(slog," %d high by %d wide, max pixel %d\n",Ph,Pw,maxval);
   plog();
   sprintf(slog," Calibration [Yaim %.3g  Xaim %.3g  Distance %.3g  FOV %.3g]\n",
	    Yaim, Xaim, Distance, FOV);  plog();

   /* Create display image array */
   picout = ppm_allocarray(Pw,Ph);
   for (i=0; i<Ph; i++)  for (j=0; j<Pw; j++)
     PPM_ASSIGN(picout[i][j],picin[i][j],picin[i][j],picin[i][j]);
 
   /* Create padded working array, clear it all to "unknown" = 0x80 */
   picpad = (unsigned char **) padmat(Ph,Pw,Pad,Pad,sizeof(unsigned char));
   for (i=-Pad; i<Ph+Pad; i++) for (j=-Pad; j<Pw+Pad; j++) picpad[i][j] = 0x80;
  
   /* Fill in image portion, fiddle any accidental "unknowns" */
   for (i=0; i<Ph; i++)  for (j=0; j<Pw; j++)
     if ((picpad[i][j] = picin[i][j]) == 0x80) picpad[i][j] = 127+(((i+j) & 1)<<1);
   
   rings(30);		/* prepare variable size spot template */

   /*****************************************************\
      fit dot templates - find spots with spot operator
   \*****************************************************/

   llog("\n* Applying spot operator at every pixel *\n");

   time1 = clock() ;  /* time it too - this is the expensive part */

   paintspots(SPOTRADIUSMIN,12);  /* apply spot operator */

   time2 = clock() ;
   sprintf(slog,"Spot finder time: %g s\n", (time2 - time1)/1000000.0); plog();
   
   showspots(4,10);	/* display the results of the spot operator */
   fp = fopen(strcat(img,".spots.ppm"),"w"); /* and write out display */
   img[imgl] = 0;
   ppm_writeppm(fp,picout,Pw,Ph,255,0);
   fclose(fp);

   /************************************************************\
      link up found spots (identified by local max of spot
      operator) into their natural grid + center spot
   \************************************************************/

   
   llog("\n* Finding local maxima of spot operator ");
   rmax = 18;  /* consider spots up to 18 pixels in radius */
   for (i= -Sad; i<Ph+Sad; i++) for (j=-Sad; j<Pw+Sad; j++)
     if ((v=flP[i][j])>0.0)
       for (k=1; k<sprpos[rmax+1]; k++)
	 if ((ii = i + spi[k]) >= 0) if (ii<Ph)
	   if ((jj = j + spj[k]) >= 0) if (jj<Pw)
	     if (flP[ii][jj] <= v) flP[ii][jj] = 0.0;  /* erase non-maxima */

   /* count maxima, to size spot list arrays */
   Nspots = 0;
   for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) if (flP[i][j]>0.0) Nspots++;

   sprintf(slog,"(%d of them) *\n",Nspots); plog();

   spotmaxlink();  /* link up spot maxima */

   /* display rings of appropriate size around the found spots */

   for (i=0; i<Nspots; i++)
     {  ii = Ispots[i] + 0.5;  jj = Jspots[i] + 0.5;
	ringdraw(i==Cspots?3:9,ii,jj,k=chP[ii][jj]);  };

   fp = fopen(strcat(img,".rings.ppm"),"w"); /* and write out display */
   img[imgl] = 0;
   ppm_writeppm(fp,picout,Pw,Ph,255,0);
   fclose(fp);

   /* display links between spots determined to be adjacent */

   for (i=0; i<Nspots; i++) if (i != Cspots)
     for (j=1; j<=4; j++) if ((k=DPspots[i][j])>=0)
       stroke(j,Ispots[i],Jspots[i],
	      (int) (Ispots[k]+Ispots[i]+1)/2,
	      (int) (Jspots[k]+Jspots[i]+1)/2);

   fp = fopen(strcat(img,".links.ppm"),"w"); /* and write out display */
   img[imgl] = 0;
   ppm_writeppm(fp,picout,Pw,Ph,255,0);
   fclose(fp);


   /***********************************************************\
     fit distortion curve, and find center to minimize scatter
   \***********************************************************/

   llog("\n* Determining optical center and radial distortion *\n");

   /* choose initial center, in spot coords, and radial polynomial degree */
   CalibSi = IIspots[Pspots];  CalibSj = JJspots[Pspots];  CalibPN = DISTORPOLYDEG ;
   CalibAS = 1.0;  /* initial guess of aspect ratio */
   sprintf(slog,"Fit polynomial degree %d\n",CalibPN-1); plog();
    
   graphout = pgm_allocarray(Pw,Ph);  /* make raster for scatter graph */

   for (dpass = -1; dpass <= 0; dpass++) /* first guess, then fit */
     { 
	 int scan;   /* extent of search */
	 float reslo, reshi;
 
	 /* search for a better image center */
	 bestfit = 1e10;  /* Any answer is better than none! */
	 bestisc = CalibSi;  bestjsc = CalibSj;  bestaspect = CalibAS;
       
	 scan = dpass?0:2;    /* 1st pass: use initial guess */
	 reslo = 1.5;  reshi = dpass?reslo:0.00001; /* 2nd pass: optimize */

	 for (res = reslo; res >= reshi; res /= 3.0)
	   {  
	       for (l = -scan; l<=scan; l++)
		 for (i = -scan; i<=scan; i++) for (j=-scan; j<=scan; j++)
		   {   /* convert to pixel coords */
		       aspect = bestaspect + l*res/10.0;
		       ipc = isc = bestisc + i*res;
		       jpc = jsc = bestjsc + j*res;
		       rootaspect = sqrt(aspect);
		       spotpixelcoord(&ipc,&jpc);
		       for (k=0; k<Nspots; k++)
			 {   /* calculate pixel and spot radii */
			     di = (Ispots[k] - ipc)*rootaspect; 
			     dj = (Jspots[k] - jpc)/rootaspect; 
			     IJspots[k] = sqrt(di*di+dj*dj);
			     di = IIspots[k] - isc; 
			     dj = JJspots[k] - jsc;
			     JIspots[k] = sqrt(di*di+dj*dj); };
		       di = fitpoly(CalibPN,Nspots,JIspots,IJspots,Poly);
		       if (di<bestfit)
			 { /* save new best, and its polynomials */
			     bestfit = di;  CalibAS = aspect;
			     CalibSi = isc;  CalibSj = jsc; 
			     CalibPi = ipc;  CalibPj = jpc;
			     for (m=0; m<CalibPN; m++) CalibP[m] = Poly[m];  
			     dj = fitpoly(CalibPN,Nspots,IJspots,JIspots,CalibS);
			 };
		   };
	       bestisc = CalibSi;  bestjsc = CalibSj; bestaspect = CalibAS;
	   };

	 /* recalculate pixel and spot radii of best fit, and 
	    find maximum of each for scaling scatter graph */
	 ddi = ddj = 0.0;  rootaspect = sqrt(CalibAS);
	 for (k=0; k<Nspots; k++)
	   {   
	       di = (Ispots[k] - CalibPi)*rootaspect; 
	       dj = (Jspots[k] - CalibPj)/rootaspect;
	       if ((IJspots[k] = sqrt(di*di+dj*dj))>ddi)  ddi = IJspots[k];
	       di = IIspots[k] - CalibSi; 
	       dj = JJspots[k] - CalibSj;
	       if ((JIspots[k] = sqrt(di*di+dj*dj))>ddj)  ddj = JIspots[k];		    
	   };

	 sprintf(slog,"aspect %.4g, center S[%.4g %.4g]==P[%.4g %.4g]",
		 CalibAS,CalibSi,CalibSj,CalibPi,CalibPj); plog();
	 sprintf(slog,"  -->  %.4g pixel rms error\n",bestfit); plog();
	 llog("S->P Polynomial: ");
	 for (i=0; i<CalibPN; i++)  { sprintf(slog,"  %.4g",CalibP[i]); plog(); }
	 llog("\n");  llog("P->S Polynomial: ");
	 for (i=0; i<CalibPN; i++)  { sprintf(slog,"  %.4g",CalibS[i]); plog(); }
	 llog("\n");

	 if (dpass) crosshair(1,ipc,jpc,10);  /* draw in initial guess of lens axis */
	 else crosshair(2,ipc,jpc,20);	/* draw in best fit lens axis */

	 /* clear scatter graph */
	 for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) graphout[i][j] = 255;

	 /* draw reference diagonal line */
	 for (j=0; j<Pw; j+=3)
	   graphout[Ph-1-(j*Ph)/Pw][j] = 128;


	 /* draw spot/pixel radius scatter points */
	 for (k=0; k<Nspots; k++)
	   {  ii = (IJspots[k]/ddi)*(Ph-4);
	      jj = (JIspots[k]/ddj)*(Pw-4);
	      for (i=0; i<4; i++) for (j=0; j<4; j++)
		graphout[Ph-1-ii-i][jj+j] = 0;
	  };
       
	 fp = fopen(strcat(img,dpass?".guess.pgm":".fit.pgm"),"w");
	 img[imgl] = 0;
	 pgm_writepgm(fp,graphout,Pw,Ph,255,0);       /* and write out graph */
	 fclose(fp);

     };
 
   /* At this point, [Ispots,Jspots] holds pixel coordinates of spots
                    [IIspots,JJspots] holds spot coordinates of spots
		    IJspots holds pixel radius from optical axis for each spot
		    JIspots holds spot radius from optical axis for each spot   */

   /*****************************************************************\
       Determine angle from plumb of radially corrected spot grid 
   \*****************************************************************/

   llog("\n* Determining angle of grid from horizontal (vertical) *\n");
     { 
	 double x, y, Sx, Sy, Sxx, Syy, Sxy, A, B;
	 double rratio;
	 double Cmin, Cmax, Cur, t;
	 int pass;

	 /* clear scatter graph */
	 for (i=0; i<Ph; i++) for (j=0; j<Pw; j++) graphout[i][j] = 128;

	 Sxx = Syy = Sxy = 0.0;  /* quadratic sums, for angle solution */
	 for (pass = 0; pass <= 1; pass++)  /* do rows, then columns */
	   {
	       /* find bounds of spot coordinates */
	       Cmin = Cmax = pass?JJspots[0]:IIspots[0];
	       for (k=0; k<Nspots; k++)
		 {   t = pass?JJspots[k]:IIspots[k];
		     if (t<Cmin) Cmin = t;  if (t>Cmax) Cmax = t;  };

	       llog(pass?"Cols":"Rows"); sprintf(slog,"[%g:%g] ",Cmin,Cmax); plog();
	       for (Cur = Cmin; Cur <= Cmax; Cur += 0.5)
		 {   n = 0;  Sx = Sy = 0;
		     for (k=0; k<Nspots; k++) /* survey, and compute means */
		       if ((pass?JJspots[k]:IIspots[k]) == Cur)
			 {   /* project spot radius onto pixel angle,
				thus removing radial distortion, but
				keeping rotation */
			     rratio = JIspots[k]/IJspots[k];
			     x = (Jspots[k]-CalibPj)*rratio;
			     y = (Ispots[k]-CalibPi)*rratio;
			     Sx += x;  Sy += y;  n++;  };
		     if (n>3) /* if row/column has enough members, add in its sums */
		       {   
			   sprintf(slog,"%d ",n); plog();
			   Sx /= n;   Sy /= n;
			   for (k=0; k<Nspots; k++) /* subtract means, compute sums */
			     if ((pass?JJspots[k]:IIspots[k]) == Cur)
			       {   /* project spot radius onto pixel angle,
				      thus removing radial distortion, but
				      keeping rotation */
				   rratio = JIspots[k]/IJspots[k];
				   x = (Jspots[k]-CalibPj)*rratio;
				   y = (Ispots[k]-CalibPi)*rratio;
				   x -= Sx;  y -= Sy;
				   if (pass) { t = -x; x = y; y = t;};
				   i = y*Pw/20 + Ph/2;  j = x*Pw/20 + Pw/2;
				   if (i>=0 && i < Ph && j>=0 && j<Pw)
				   graphout[i][j] = pass?255:0;
				   Sxx += x*x;  Syy += y*y;  Sxy += x*y;	       
			       };
		       };
		 };
	       llog("\n");
	   };
	 A = Sxy;   B = Syy - Sxx;  /* closed form solution, in terms of sums */

	 fp = fopen(strcat(img,".tilt.pgm"),"w");
	 img[imgl] = 0;
	 pgm_writepgm(fp,graphout,Pw,Ph,255,0);       /* and write out graph */
	 fclose(fp);

	 CalibAN = atan2(-B+(B<0?1:-1)*sqrt(B*B+4.0*A*A),2.0*A);
	 /* out of all the minimum, maximum, flipped, solutions, pick the one
	    that requires the least rotation of the original grid */
	 while (CalibAN>PI/4) CalibAN -= PI/2;
	 while (CalibAN<-PI/4) CalibAN += PI/2;
		 
	sprintf(slog,"Grid angle = %.4g degrees\n",CalibAN*180/PI); plog();

     };

   /* Write out all calibration parameters into .calib file */
   llog("\n* Writing out calibration file *\n");

   fp = fopen(strcat(img,".calib"),"w");   img[imgl] = 0;
   /* write out the calibration setup, 
      Ph, Pw, Xaim, Yaim, Distance, FOV,
      The polynomial degree+1,
      the distortion center i and j pixel coordinates, the aspect ratio,
      the distortion center i and j spot coordinates, the image rotation angle,
      the Spot->Pixel polynomial coefficients,
      the Pixel->Spot polynomial coefficients */
   
   fprintf(fp,"%d %d %.6g  %.6g  %.6g  %.6g\n", Ph, Pw, Yaim, Xaim, Distance, FOV);
   fprintf(fp,"%d %.6g %.6g %.6g %.6g %.6g %.6g\n", 
	   CalibPN,CalibPi, CalibPj,CalibAS,CalibSi,CalibSj,CalibAN);
   for (i=0; i<CalibPN; i++) fprintf(fp,"%.6g ",CalibP[i]); fprintf(fp,"\n");
   for (i=0; i<CalibPN; i++) fprintf(fp,"%.6g ",CalibS[i]); fprintf(fp,"\n");
   fclose(fp);
   
   fp = fopen(strcat(img,".center.ppm"),"w"); /* and write out display of centers */
   img[imgl] = 0;
   ppm_writeppm(fp,picout,Pw,Ph,255,0);
   fclose(fp);

   llog("\n* Rectifying image *\n");
   /* Rectify image  */
   { 
       double ip, jp, rp, is, js, rs, ang, sang, cang;
       int deg, i, j, k, ii, jj, errc;
       double SpotSpacing;
	     
       /* Desired spot spacing calculated from user FOV request and Distance */
       SpotSpacing = Pw/(Distance*2.0*tan(FOV*PI/360.0));

       sprintf(slog,"Rectified image spot spacing %.4g pixels\n",SpotSpacing); plog();
       sprintf(slog,"Calibration parameters:\n"); plog();
       sprintf(slog,"Ph %d Pw %d Yaim %.4g  Xaim %.4g  Distance %.4g  FOV %.4g\n",
	    Ph, Pw, Yaim, Xaim, Distance, FOV);  plog();
       sprintf(slog,"ip %.4g  jp %.4g  aspect %.4g  is %.4g js %.4g angle %.4g\n",
	      CalibPi,CalibPj,CalibAS,CalibSi,CalibSj,CalibAN); plog();
       llog("S->P Polynomial:");
       for (i=0; i<CalibPN; i++)
	 { sprintf(slog,"  %.4g",CalibP[i]); plog(); }
       llog("\n");   llog("P->S Polynomial:");
       for (i=0; i<CalibPN; i++)
	 { sprintf(slog,"  %.4g",CalibS[i]); plog(); }
       llog("\n");

       deg = CalibPN;   ipc = CalibPi;     jpc = CalibPj;
       aspect = CalibAS;   isc = CalibSi;  jsc = CalibSj;

       rootaspect = sqrt(aspect);
       errc = 0;

       /* Make grid spacing SpotSpacing pixels between spot centers */

       for (i=0; i<Ph; i++)  for (j=0; j<Pw; j++)
	 {   
	     /* calculate spot coordinates desired for this destination pixel */
	     is = (i - Ph/2)/SpotSpacing + Yaim;
	     js = (j - Pw/2)/SpotSpacing + Xaim;

	     /* calculate radius from spot optical axis */
	     is -= isc;    js -= jsc;
	     rs = sqrt(is*is + js*js);

	     /* transform spot radius to pixel radius */
	     rp = CalibP[deg-1]; for (k = deg-2; k>=0; k--)  rp = rp*rs+CalibP[k];

	     /* project pixel radius back onto to spot radius direction,
	        rotated by grid angle from horizontal  */
	     ang = atan2(is,js);
	     sang = sin(ang-CalibAN);  cang = cos(ang-CalibAN);

	     ip = rp*sang/rootaspect;
	     jp = rp*cang*rootaspect;
	
	     /* and add pixel optical axis to get source pixel */
	     ii = ip + ipc;     jj = jp + jpc;

	     if (ii<0 || ii>Ph-1 || jj<0 || jj>Pw-1)
	       {  errc++; PPM_ASSIGN(picout[i][j],128,128,128); }
	     else
	       PPM_ASSIGN(picout[i][j],picin[ii][jj],picin[ii][jj],picin[ii][jj]);
	 };
       sprintf(slog,"%d pixels from beyond the edge\n",errc); plog();
       
       for (k=-1; k<=1; k+=2)
       {
       for (is=0; is<Ph/2; is+=SpotSpacing)
	 { i = is + 0.5;  stroke(i?4:2,k*i+Ph/2,0,k*i+Ph/2,Pw-1); };
       for (js=0; js<Pw/2; js+=SpotSpacing)
	 { j = js + 0.5;  stroke(j?4:2,0,k*j+Pw/2,Ph-1,k*j+Pw/2); };
       };

   fp = fopen(strcat(img,".rect.ppm"),"w"); /* and write out display */
   img[imgl] = 0;
   ppm_writeppm(fp,picout,Pw,Ph,255,0);
   fclose(fp);
   }

   fclose(flog);  /* close log file */
   return(0);
};